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INTRODUCTION 


Research  under  this  contract  began  as  scheduled  on  October  1,  1981. 

During  the  first  six  months,  no  significant  deviations  from  the  plan 
contained  in  our  proposal  dated  May  13,  1981,  occurred.  Research  performed 
relative  to  each  of  the  tasks  listed  in  our  Statement  of  Work,  dated  September 
21,  1981,  is  summarized  in  the  following  paragraphs.  Further  details  regarding 
some  of  these  topics  are  provided  in  the  attached  appendices  and  in  videotapes 
previously  furnished  to  DARPA. 


RESEARCH  ACCOMPLISHED 

Task  1;  Vertical  Sensor  Evaluation 

Rough  terrain  locomotion  on  a  laboratory  terrain  board  has  been  accomplished 
by  the  OSU  Hexapod  Vehicle.  A  surplus  vertical  gyro  furnished  by  the  University 
Hsf  Wisconsin  was  used  for  this  purpose.  The  results  obtained  are  dociuented 
in  a  videotape  entitled  "The  Adaptive  Suspension  Vehicle  Concept,"  dated 
March,  1982.  While  the  outcome  of  this  test  was  generally  satisfactory,  a 
noticeable  amount  of  gyro  drift  occurred  during  trials  involving  large  amounts 
of  body  motion.  An  alternative  gyro  has  been  ordered,  and  further  tests 
will  be  made  during  the  coming  quarter  to  determine  if  this  effect  can  be 
reduced.  Preliminary  evaluation  of  pendulum  sensors  indicates  that  these 
devices  are  excessively  affected  by  vehicle  body  motion  during  locomotion, 
but  may  be  useful  as  an  inexpensive  means  for  verification  of  gyro  function 
during  automatic  checkout  of  vehicle  subsystems  prior  to  initiation  of  gait. 


A  simulation  study  involving  a  CRT  display  of  a  simplified  ASV-84 
hexapod  vehicle  has  been  initiated  for  the  purpose  of  studying  alternative 
control  modes.  Research  thus  far  has  centered  about  a  "close  maneuvering" 
mode  in  which  signals  from  a  three-axis  joystick  are  used  to  control  fore-aft 
velocity,  right-left  velocity,  and  turning  rate  about  the  vehicle  vertical 
axis.  In  this  control  mode,  the  vertical  velocity  of  the  body  is  automatically 
adjusted  to  maintain  a  desired  altitude  above  the  terrain.  The  remaining  two 
components  of  body  velocity,  namely,  pitch  rate  and  roll  rate,  are  also 
automatically  controlled,  either  to  maintain  the  body  at  an  operator  specified 
elevation  and  roll  angle,  or  else  to  adjust  these  angles  automatically  to 
keep  the  vehicle  body  parallel  to  the  average  terrain  slope.  Exanples  of 
results  obtained  with  this  simulation  are  included  in  videotapes  previously 
submitted  to  OARPA.  Research  on  this  topic  during  the  next  qiiarter  of  this 
contract  will  be  aimed  at  definition  of  additional  control  modes,  improvement 
of  the  existing  simulation,  and  tests  of  the  above  described  "terrain¬ 
following"  locomotion  with  the  OSU  Hexapod. 

Task  3;  Insect  Control  Studies 

A  simulation  program  using  a  "free  gait"  algorithm  has  been  written. 

This  program  produces  nonperiodic  stepping  patterns  and  is  capable  of 
dealing  with  terrain  possessing  a  significant  percentage  of  regions  unsuitable 
for  load  bearing  due  to  the  presence  of  holes,  rocks,  soft  soils,  etc.  The 
results  of  this  simulation  will  be  compared  to  digital  and  film  recordings 
of  locust  stepping  patterns  to  be  provided  by  the  University  of  Alberta. 
Preliminary  data  of  this  type  has  already  been  obtained  from  Dr.  Robert  Franklin 


of  the  University  of  Oregon.  The  purpose  of  this  investigation  is  to  develop 
more  versatile  algorithms  for  automatic  control  of  ASV-84  stepping  patterns 
during  complex  maneuvering  over  rough  terrain. 

Task  4;  Fault  Tolerant  Software 

The  definition  of  this  task  has  been  expanded  to  include  automatic 
pre-locomotion  checkout  of  vehicle  subsystems  and  on-line  detection  of  stepping 
pattern  errors  which  could  result  in  any  of  the  following  three  undesirable 
conditions:  1)  motion  of  a  joint  to  an  angle  such  that  its  kinematic  limit 
is  encountered;  2)  collision  of  adjacent  limbs;  3)  instability  of  a  support 
pattern  or  possible  collision  of  the  body  with  the  ground'for  any  other 
reason.  With  software  now  being  written,  imminent  violation  of  any  of  these 
"safety"  conditions  will  automatically  halt  the  vehicle  and  will  provide  the 
operator  with  an  error  message.  It  is  anticipated  that  this  software  will 
be  completed  and  tested  with  the  OSU  Hexapod  Vehicle  during  the  fourth 
quarter  of  the  present  contract  year. 

Task  5:  Laser  Foothold  Designator 

One  television  array  camera  has  been  delivered.  This  camera  has  been 
successfully  interfaced  to  the  PDP-11/70  computer.  Camera  calibration  is 
under  way  and  should  be  completed  during  the  next  quarter  of  research. 

Delivery  of  the  second  camera,  needed  for  triangulation  on  the  operator 
controlled  laser  spot  reflected  from  the  terrain,  is  also  expected  in  the 
next  quarter.  Software  for  optical  control  of  stepping  patterns  for  the  OSU 
Hexapod  Vehicle  has  been  completed  and  tested  using  keyboard  entry  of  terrain 
coordinates  to  simulate  the  range-finding  function. 
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Task  6:  Leg  Geometry  Studies 

Assembly  of  the  DUWE  (Dynamic  Unpowered  Walking  Experiment]  was  completed 
and  the  device  walked  successfully.  The  measured  specific  resistance  was 
in  the  range  0.1  to  0.2  which  is  comparable  to  that  of  an  automobile.  The 
machine  behavior  also  correlated  well  with  that  predicted  by  computer  simulation. 
We  believe  that  this  experiment  demonstrates  that  there  is  no  instrinsic 
mechanical  obstacle  to  the  achievement  of  low  specific  resistances  by  fully 
terrain-adaptive  walking  machines. 

Computer-aided  geometric  design  of  leg  geometry  continued  with  feedback 
from  both  the  DUWE  experiment  and  the  Monopod  (fully  discussed  in  an  appendix 
to  the  final  report  on  Contract  no.  MDA903-81-C-0138] .  Dimensions  were 
finalized  for  the  geometry  based  on  a  four-bar  linkage  with  a  sliding  shank. 
Parallel  geometric  studies  of  pantograph  leg  geometries  were  conducted  and 
a  set  of  dimensions  for  a  leg  of  this  type  were  also  frozen. 

Finite  element  studies  of  stress  and  deflection  in  legs  of  the  two 
types  mentioned  above  have  been  initiated.  These  studies  are  to  be  coordinated 
with  the  frame  model  to  produce  a  stress  and  vibration  model  of  the  whole 
vehicle. 

Task  7:  Power  Package  Specifications 

A  preliminary  set  of  pump  specifications  has  been  transmitted  to  the 
University  of  Wisconsin.  The  general  configuration  of  the  flywheel  package 
has  been  agreed  upon  and  is  discussed  in  Appendix  1.  A  specification  for 
a  generator  or  alternator  awaits  estimates  of  computer  and  instrumentation 
power  requirements. 
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Task  8:  Power  Distribution  and  Actuator  Servo  Design 

A  first  version  of  the  power  transmission  system  configuration  design 
has  been  completed  and  is  discussed  in  Appendix  1.  Simulation  studies  of 
portions  of  this  system  are  in  progress  and  representative  results  are 
discussed  in  Appendix  2.  A  kinetostatic  simulation  of  the  whole  vehicle 
initiated  to  provide  actuator  loads,  actuator  velocities,  and  reflected 
inertias  for  input  to  the  transmission  system  simulation  is  being  further 
developed  to  simulate  arbitrary  paths  on  sloping  and  uneven  terrain. 

Component  procurement  for  the  breadboard  leg  system  is  in  progress. 

As  of  the  end  of  the  time  period  covered  by  this  report,  the  variable 
displacement  pumps  and  control  valves  have  been  delivered.  The  actuators 
have  been  selected  and  studies  are  in  progress  for  selection  of  a  suitable 
microcomputer  configuration. 

Task  9;  Frame  Design 

As  of  the  end  of  the  first  six  months  of  this  research,  a  lightweight 
mock-up  of  the  vehicle  frame  has  been  constructed.  Field  mobility  tests 
using  this  frame  supported  by  three  people  are  scheduled  to  take  place  in 
the  coming  quarter. 

A  preliminary  configuration  for  the  vehicle  frame  structure  has  been 
selected  and  a  structural  model  for  weight  distribution  studies  and  for 
finite  element  input  has  been  generated  using  the  SUPERTAB  program. 
Preliminary  finite  element  analysis  has  been  performed  using  the  SUPERB 
program.  Material  from  these  studies  in  included  in  the  videotape  delivered 
as  part  of  the  final  report  on  Contract  MDA903-81-C-0138.  On  the  basis  of 
these  preliminary  studies,  and  as  a  result  of  experience  with  the  OSU  Hexapod 
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and  other  robot  structure  designs,  it  appears  that  the  primary  design  constraints 
will  be  natural  frequencies  and  modes  of  structural  vibration  rather  than 
load-bearing  capacity.  Design  modelling  is  proceeding  on  this  asstunption. 


Task  10:  On-Board  Computer  Design 

An  Intel  8086/8087  single  board  computer  has  been  interfaced  to  the 
electric  monopod  vehicle.  Data  conversion  (A  to  D  and  D  to  A)  is  provided 
through  an  Intel  8088  computer  which  has  also  been  interfaced  to  this  machine. 
These  two  computers  communicate  via  the  Intel  Multibus,  which  includes 
hardware  arbitration  of  conflicting  bus  requests.  A  program  for  single  axis 
control  of  the  monopod  is  being  written  in  PASCAL  and  assembly  language  using 
a  Hewlett-Packard  64000  Microprocessor  Development  System  (MDS) .  Although  it 
is  anticipated  that  this  program  will  be  completed  during  the  next  quarter 
of  research  under  this  contract,  this  approach  to  programming  is  awkward 
and  inefficient.  It  has  been  decided  that  it  would  be  much  better  to  make 
use  of  an  Intel  MDS  and  a  suitable  model  number  has  been  determined.  While 
this  item  is  included  in  the  budget  for  the  second  year  of  this  project,  its 
need  is  strongly  felt,  and  permission  for  purchase  during  the  current  contract 
year  has  been  requested  from  our  local  contract  monitor. 

Task  11:  Software  Design 

Version  3.4  of  the  OSU  Hexapod  real-time  software  has  been  completed. 

A  listing  is  included  with  this  report  as  Appendix  3.  This  version  has  been 
modularized  in  such  a  way  as  to  be  conveniently  adapted  to  a  multiprocessor 
implementation.  Among  the  other  important  features  of  this  software  are  its 
improved  on-line  operator  communication  characteristics  and  the  inclusion  of 
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a  rough  terrain  locomotion  capability.  Test  results  obtained  with  this 
software  in  conjunction  with  the  OSU  Hexapod  Vehicle  are  documented  in  the 
two  videotapes  previously  referenced  in  this  report. 

Task  12:  Electronic  Subsystem  Design 

A  study  of  sensor  requirements  for  the  ASV-84  Vehicle  has  been  initiated. 
Requirements  for  leg  servo  power  electronics  are  being  developed.  Design  of  an 
ultrasonic  ranging  system  for  possible  use  as  a  leg  proximity  sensor  has  been 
completed.  Components  have  been  ordered,  and  construction  of  a  breadboard 
model  proximity  sensing  subsystem  is  scheduled  for  the  coming  quarter. 
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SUMMARY 


This  report  summarizes  work  accomplished  during  the  first  six  months 
of  a  three-year  contract  leading  to  field  testing  of  an  experimental  adaptive 
suspension  vehicle  (ASV-84).  The  key  feature  of  this  vehicle  is  that  it  is 
supported  and  propelled  by  an  actively  controlled  linkage  mechanism  enabling 
it  to  adjust  its  configuration  to  ground  contours  in  order  to  smooth  body 
motion,  improve  slope  climbing  and  rough  terrain  mobility,  and  potentially, 
to  achieve  better  fuel  economy  tlian  conventional  off -road  vehicles.  The 
research  is  motivated  by  the  observation  that  approximately  one-half  of  the 
land  surface  of  the  earth  is  presently  inaccessable  to  any  conventional  wheeled 
or  tracked  vehicle,  while  man  and  large  animals  are  able  to  traverse  such 
areas  with  relative  ease. 

The  first  year  of  this  program  is  devoted  to  design  of  the  ASV-84 
machine.  It  now  appears  certain  that  this  vehicle  will  be  a  hexapod  walking 
machine  without  rolling  elements  in  contact  with  the  ground,  although  the 
design  will  be  such  that  a  retrofit  of  such  components  will  be  possiblll  after 
initial  field  tests.  The  1984  vehicle  will  be  provided  with  advanced  sensing 
including  a  terrain-scanning  laser  ranging  device,  proximity  sensors  and  force 
sensors  on  each  leg,  and  an  inertial  measurement  unit.  This  system  will  carry 
a  single  operator  who  will  be  responsible  for  route  selection,  speed  control, 
and  other  high  level  decisions.  Lower  levels  of  coordination  and  processing 
of  sensor  data  will  be  handled  by  a  multiprocessor  on-board  computer. 

In  order  to  conduct  the  design  phase  of  this  research  in  a  realistic 
context,  a  number  of  experimental  machines  are  being  used.  Included  in  these 
are:  1)  the  OSU  Hexapod  Vehicle,  a  functioning  computer-controlled  quarter- 
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scale  laboratory  model  walking  machine;  2)  the  Electric  Monopod,  a  wheeled 
test-bed  vehicle  propelled  and  supported  by  a  quarter-scale  linkage-coordinated 
leg;  and  3)  the  DUWE  vehicle,  a  towable  unpowered  hexapod  designed  to  study 
resonant  energy  storage  during  limb  cycling  in  order  to  improve  locomotion 
efficiency  to  a  level  comparable  to  that  achieved  by  wheeled  vehicles. 

Due  to  the  complexity  of  this  project,  and  to  the  fact  that  the  research 
has  to  do  with  control  of  motion,  videotaping  has  been  chosen  as  the  primary 
means  of  reporting  experimental  results  to  DARPA.  The  present  document 
therefore  represents  only  a  summary,  and  access  to  the  videotapes  is  required 
in  order  to  obtain  a  full  understanding  of  research  accomplished  during  this 
reporting  period. 

In  conclusion,  no  major  deviations  from  the  original  project  plan  have 
occurred  during  the  first  six  months  of  this  research.  We  currently  anticipate 
that  the  design  of  the  ASV-84  vehicle  will  be  completed  on  schedule  at  the 
end  of  the  first  year  of  research.  A  major  design  review  is  presently 
scheduled  for  October,  1982,  and  it  is  anticipated  that  vehicle  construction 
will  begin  immediately  after  this  review. 
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Appendix  1 

To  be  presented  at  17th  Intersociety  Energy  Conversion  Engineering  Conference,  Los 
Angeles,  August  8-13,  1982. 

THE  USE  OF  MECHANICAL  ENERGY  STORAGE  IN  AN  UNCONVENTIONAL ,  ROUGH  TERRAIN  VEHICLE 


•  t  * 

K.J.  Waldron  .  A. A.  Prank  ,  and  K.  Srinivasan 


*  Dept,  of  Mechanical  Engineering  t  Dept,  of  Electrical  Engineering 
Ohio  State  University  University  of  Wisconsin 


ABSTRACT 

Vehicles  designed  for  very  rough  terrain 
are  subject  to  large  variations  in  power  demand 
and,  therefore,  benefit  from  energy  storage  and' 
regeneration.  This  paper  concerns  energy  man¬ 
agement  and  conservation  in  a  legged  vehicle 
presently  being  designed  for  transportation  over 
such  terrain.  Both  the  rough  terrain  require¬ 
ment  and  the  oscillatory  nature  of  the  walking 
action  dictate  careful  design  for  energy  manage¬ 
ment  if  the  power  to  weight  ratio  is  to  be  im¬ 
proved  over  current  designs. 

The  arrangement  of  the  power  generation, 
storage  and  transmission  system  will  be  dis¬ 
cussed.  This  comprises  a  gasoline  engine,  fly¬ 
wheel  storage  package  and  a  hydraulic  transmis¬ 
sion  and  actuation  system  featuring  computer 
controlled  variable  displacement  pumps. 


INTRODUCTION 

The  subject  of  this  paper  is  the  energy  man¬ 
agement  system  design  of  an  unconventional  vehicle 
intended  for  service  in  very  rough  terrain.  An 
artist's  conception  of  this  vehicle  is  shown  in 
Figure  1.  It  may  be  seen  that  the  support  and  lo¬ 
comotion  system  consists  of  six  legs.  Legged  loco¬ 
motion  presents  a  ntimber  of  advantages  over  wheels 
or  tracks,  particularly  in  rough  terrain  (1).  It 
comes  as  a  surprise  to  most  people  that  only  about 
50^  of  the  earth's  land  surface  is  accessible  to 
wheeled  or  tracked  vehicles  (2) .  At  least  90%  is 
accessible  to  animals  using  legged  locomotion  and, 
in  principle,  to  artificial  legged  vehicles.  Com¬ 
mercial  machines  using  a  combination  of  wheels  and 
articulated  dimbs  (3,4)  have  demonstrated  ability 
to  handle  gradients,  soil  types  and  terrain  fea¬ 
tures  far  beyond  those  which  conventional  wheeled 
or  tracked  vehicles  can  handle. 

There  have  been  a  number  of  experimental  walk¬ 
ing  machines  built.  Those  described  in  references 
(S,6,7,8,9)  have  the  capability  to  adapt  their 
gaits  to  terrain  conditions  as  does  the  machine  we 
are  designing.  A  severe  problem  with  all  of  these 
machines,  which  has  not  previously  been  seriously 


Figure  1 ;  Concept  drawing  of  the  adaptive  suspen¬ 
sion  vehicle 


addressed,  is  energy  management.  Figure  2  is  a 
modification  of  Gabrielli  and  Von  Karman's  diagram 
(10)  showing  the  ratio  of  locomotion  power  to 
weight  plotted  against  speed  for  various  locomo¬ 
tion  systems.  Two  points  have  been  added.  These 
correspond  to  the  G.E.  Quadruped  (5)  and  the  OSU 
Hexapod  (6)  and  are  representative  of  fully  adap¬ 
tive  walking  machines.  It  may  be  seen  that  an  im¬ 
provement  of  at  least  one  order  of  magnitude  in 
power  to  weight  ratio  is  necessary  for  these  to 
become  competitive  with  tracked  vehicles,  and 
two  orders  of  magnitude  improvement  is  needed  to 
approach  the  performance  of  biological  walking 
systems. 

A  discussion  of  the  power  loss  mechanisms  in 
walking  machines  may  be  found  in  reference  (11). 
Both  the  vertical  plane,  "mammalian"  type  leg 


Figure  2:  Specific  power,  defined  as  the  ratio 
of  the  maximum  power  available  to 
the  gross  weight  of  the  vehicle, 
plotted  as  a  function  of  its  maximum 
speed  (from  Gabrielli  and  von  Kaiman, 
19S4) 


geometry  used,  as  distinct  from  the  "arthropod" 
type  leg  geometry  of  the  machines  described  in 
references  (6, 7, 8, 9),  and  the  unusual  linkage 
geometry  (Figure  3)  were  adopted  for  energy  man¬ 
agement  reasons.  The  selection  of  the  leg  geo¬ 
metry  is  further  described  in  reference  (12) . 

Any  vehicle  encounters  wide  variations  in 
power  demand  resulting  from  changing  terrain  and 
speed  conditions.  For  energy  conservation  pur¬ 
poses  it  is,  therefore,  attractive  to  use  energy 
storage  and  regeneration  on  a  vehicle  to  allow 
optimization  of  the  engine  operating  conditions 
and  to  regeneratively  recover  energy  absorbed 
when  braking  (13).  The  more  severe  the  terrain, 
the  more  extreme  the  variation  in  power  demand. 
The  reasons  for  electing  to  make  use  of  an  energy 
storage  device  in  the  vehicle  we  are  designing 
are,  therefore; 

1 .  The  need  for  substantial  Improvement  in 
power  deiaand. 

2.  The  need  for  spurts  of  very  high  power 
to  overcome  severe  obstacles  in  a  system 
which  must  also  be  highly  efficient  on 
level  terrain. 

For  reasons  of  actuator  force  to  weight  ra¬ 
tio  and  of  the  simplicity  and  quick  response  of 
direct  drive  without  gears  or  other  speed  redu¬ 
cers,  a  hydraulic  power  transmission  and  actua¬ 
tion  system  will  be  used.  It  Is  the  configura¬ 
tion  of  this  system,  and  of  the  associated  prime 


Figure  3;  Concept  drawing  of  adaptive  suspension 
vehicle  leg 


mover  and  energy  storage  package,  which  forms  the 
subject  of  this  paper. 

PRIME-MOVER  AND  ENERGY -STORAGE  PACKAGE 
Requirements 

For  any  vehicle  there  are  basic  requirements 
that  a  power  supply  must  meet.  These  are  sustained 
expected  power  and  peak  power.  The  sustained  or 
steady-state  condition  sets  the  waste-energy  re¬ 
quirements.  The  peak -power  requirements  set  the 
component  size  (i.e.,  torque  and  speed  of  each  com¬ 
ponent)  . 

As  an  example,  for  standard  automobiles  the 
average  power  required  to  drive  the  vehicle  in  the 
city  is  about  3  to  4  hp  for  a  vehicle  about  our 
size.  The  peak  power,  however,  is  about  45  hp. 

The  vehicle,  however,  may  be  constructed  with  60 
hp  or  more.  The  reason  for  this  disparity  is  that 
the  maximum  available  power  is  seldom  used.  The 
power  train,  however,  snist  be  designed  to  handle 
the  peak,  but  it  nay  not  be  designed  to  sustain 
this  peak.  We  will  need  a  peak-power  capability  10 
to  20  times  the  average.  Me  can  readily  meet  this 
requirement  with  a  flywheel  energy-storage  power 
plant. 

Energy-storage  capacity  is  another  design 
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factor  of  the  power  supply.  Standard  power  sys¬ 
tems  do  not  have  large  energy  storage  In  the  prime 
mover.  The  stored  energy  is  usually  in  the  form 
of  liquid  fuel.  Fuel  flow  is  required  to  increase 
power.  These  systems,  however,  cannot  provide  the 
same  efficiency  of  conversion  for  all  levels  of 
power.  In  fact,  the  efficiency  of  conversion 
drops  as  the  power  level  drops.  In  addition,  it 
is  not  possible  to  regenerate  energy,  i.e.,  feed 
vehicle  energy  back  to  the  power  plant  for  later 
re-use.  This  implies  that  we  waste  energy  every 
time  the  vehicle  is  stopped  faster  than  by  coast¬ 
ing  and  every  time  we  go  down  a  steep  hill. 

To  provide  a  power  supply  which  can  provide 
a  peak  power  of  about  150  hp,  a  sustained  power 
of  60  hp,  and  an  energy- storage  capacity  of  about 
1/3  hp-hr,  an  engine-flywheel  system  is  considered. 
The  requirement  for  energy  storage  is  determined 
by  the  amount  of  energy  recoverable  going  down  a 
grade  (of  60%  for  100  yards]  or  the  time  for  sus¬ 
tained  high  power.  In  the  above  specification,  ' 
we  would  be  able  to  sustain  150  hp  for  about  10 
seconds.  This  amount  of  power  will  allow  the 
vehicle  to  lift  itself  up  at  20  feet  per  second. 

Of  course,  to  achieve  this  performance,  all  com¬ 
ponents  must  be  sized  to  handle  this  rate  of 
energy  flow.  This  capacity  allows  the  vehicle  to 
climb  vertically  200  feet  in  10  seconds  if  so  de¬ 
sired  or  required. 

The  power-plant  package  is  shown  in  Figure  4. 
This  package  shows  a  design  which  can  be  relative¬ 
ly  easily  implemented.  The  flywheel  package  has 
two  clutches.  An  engine  clutch  allows  it  to  be 
decoupled  from  the  flywheel  supply  and  the  varia¬ 
ble-displacement  drive  pump.  The  purpose  of  the 
second  clutch  is  to  start  the  flywheel  automa¬ 
tically  without  having  to  control  the  slip  of  the 
clutch. 


Figure  4;  Power  plant  package 


When  a  flywheel  power  plant  is  first  started, 
the  engine  must  engage  the  flywheel  gradually  to 
prevent  too  low  an  engine  speed  and  consequent 
engine  stall.  There  are  many  ways  to  accomplish 
this.  One  simple  way  which  has  been  tried  suc¬ 
cessfully  is  to  start  the  engine,  then  slip  the 
clutch  to  spin  the  flywheel  to  a  minimum  speed. 
This  method  can  be  accomplished  either  by  starting 
the  engine  first  or  electrically  spinning  the  fly¬ 
wheel,  then  engaging  the  engine.  The  first 
technique  can  be  accomplished  with  less  electrical 
energy  if  the  engine  is  in  good  tune.  The  second 
method  requires  gradual  application  of  throttle 
during  acceleration. 

An  alternative  which  requires  no  more  coi^- 
nents  but  provides  a  somewhat  smoother  starting 
algorithm  is  to  start  the  engine  normally,  then 
hydrostatically  accelerate  the  flywheel.  This 
technique,  though  smooth,  means  that  one  hydrau¬ 
lic  pump  must  be  always  connected  to  the  flywheel 
mechanism.  The  consequence  of  this  is  that  we 
would  always  have  its  associated  spin  loss.  If 
we  choose  one  of  the  lower-capacity  pumps  for  this 
function  the  loss  can  be  tolerable. 

The  total  flywheel  spin  loss  should  be  less  than 
1  hp  at  peak  speed.  The  largest  spin  loss  is 
expected  to  come  from  the  hydraulic  pumps.  It  is 
generally  important  to  keep  the  hydraulic  pumps 
operating  at  the  minimum  speed  to  supply  the  power 
required.  Thus,  ideally,  a  two-  or  three-speed 
transmission  should  be  used  between  the  hydrostatic 
pumps  and  the  flywheel.  This  will  allow  minimiza¬ 
tion  of  the  losses  when  the  power  requirements  are 
low.  It  further  gives  the  system  more  flexibility 
and  greater  dynamic  range. 

Design  Alternative* 

Engine  selection  is  based  on  sisiplicity  and 
power  density.  Simplicity  would  dictate  an  air¬ 
cooled  diesel  engine.  However,  power  density 
would  greatly  favor  an  air-cooled  gasoline  engine. 
Simplicity  and  maintainability  also  favor  a  gaso¬ 
line  production  engine.  Thus,  an  air-cooled  pro¬ 
duction  engine  of  about  60  hp  is  clearly  the  best 
alternative. 

The  flywheel  configuration  is  dependent  upon 
energy -density,  safety,  construction-cost,  relia¬ 
bility,  and  maintainability  trade-offs.  Two  alter¬ 
native  designs  being  evaluated  are  shown  sketched 
in  Figures  5  and  6.  These  designs  show  different 
approaches  to  the  five  trade-off  items  above. 

Figure  5  shows  a  semiconstant -stress  flywheel  with 
an  attachment  boss.  This  sort  of  design  requires 
a  larger  diameter  for  a  given  speed  or  a  higher 
speed  for  a  given  diameter.  A  physical  limit  is 
approximately  24  inches  diameter  and  a  maximum 
comfortable  operating  speed  of  about  15,000  rpm. 

The  second  possibility  is  to  be  conservative  and 
design  a  semirectangular  cross  section  with  an 
insert  hub.  This  design  has  the  advantage  of  pro¬ 
viding  absolute  safety  from  overspeed;  i.e.,  the 
flywheel  is  assembled  by  cooling  the  center  to 
liquid-nitrogen  tesiperature  and  heating  the  out¬ 
side  to  a  high  temperature.  The  interference  fit 
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Figure  S:  Alternative  flywheel  designs 
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Figure  6;  Alternative  flywheel  designs 


is  calculated  to  occur  at  20%  over  the  maximum 
speed.  An  alternative  would  be  to  assemble  the 
system  at  20%  overspeed,  i.e.,  dynamically  assemble 
the  hub  and  disc.  Overspeed  of  the  flywheel 
causes  the  disc  to  fly  off  the  hub,  thus  prevent¬ 
ing  the  overspeeding  of  the  disc.  This  20%  margin 
in  speed  is  easy  to  adhere  to,  since  20%  overspeed 
implies  a  44%  increase  in  stored  energy;  i.e., 
with  the  characteristics  of  engines,  this  over¬ 
speed  is  difficult  to  achieve  if  all  protective 
systems  fail. 

The  flywheel  systems  discussed  will  have  a 
vacuum/ lubrication  pump.  The  idea  is  sisiply  to 
use  a  scavenging  pump  to  remove  oil,  then  to  care¬ 
fully  use  atsMspheric  pressure  to  force  oil  back 
into  the  flywheel  case.  The  calibration  of  an 
orifice  and  the  use  of  constant-viscosity  oil  will 
yield  a  simple  yet  effective  combined  lubrication- 
vacuum  system.  This  pump  should  be  incorporated 
around  the  flywheel  shaft.  An  ordinary  oil  pump 
as  used  in  automatic  transmissions  can  be  utilized. 
This  pump  may  be  used  to  start  the  flywheel ;  in 
this  mode  it  really  is  a  hydraulic  motor. 


The  flywheel -engine  power  supply  is  a  rela¬ 
tively  straightforward  design  effort.  The  engine 
control  is  dependent  on  final  decisions  on  the 
placement  of  the  starter  motor.  It  is  anticipated 
that  the  engine  will  be  bu^-started  by  the  fly¬ 
wheel  and  then  the  engine  will  spin  the  flywheel 
up  at  a  fixed  throttle  until  maximum  speed  is 
reached.  At  this  speed  the  ignition  will  be  auto¬ 
matically  cut  and  the  engine  decoupled  by  the  en¬ 
gine  clutch;  the  flywheel  will  supply  the  power 
until  its  speed  drops  to  about  half  the  maximum, 
at  which  time  the  engine  is  re-engaged  and  started. 
This  on-off  operation  is  one  mode  of  operation 
possible.  A  continuous -operation  node  is,  of 
course,  also  possible,  in  which  the  engine  is 
throttled  to  maintain  a  given  flywheel  speed  less 
than  the  maximum.  This  operational  node  uses  the 
flywheel  as  a  speed  stabilizer  for  the  engine.  It 
is  not  as  efficient  as  the  on-off  operation; 
however,  it  is  simpler  to  implement. 

The  flywheel  power  supply  is  a  necessary  part 
of  this  vehicle  design,  since  high  transient 
torques  are  expected  to  be  applied  to  the  hydrau¬ 
lic  system.  These  transients  are  translated  into 
transient  or  peak  power.  Without  a  flywheel  for 
stabilization,  the  engine  would  have  to  be  extreme¬ 
ly  responsive.  The  flywheel  power  supply  allows 
us  to  use  other  forms  of  prime  Mver  as  well,  if 
they  become  available.  Power  sources  such  as  Stir¬ 
ling  engines,  turbines,  and  diesels  are  all  viable 
with  the  flywheel  as  an  intermediary. 

HYDRAULIC  SYSTEM  CONFIGURATION 

As  is  shown  in  Figure  3,  each  leg  of  the 
machine  is  to  have  three  degrees  of  freedom  each 
powered  by  a  linear  actuator.  Hydraulic  linear 
actuators  are  lighter  than  rotary  actuators  in 
equivalent  service.  The  non-linearity  of  the  re¬ 
lationship  between  actuator  displacement  and  joint 
angle  and  the  limited  range  of  joint  rotation  per¬ 
mitted  do  not  present  serious  problems  in  this 
system.  The  three  actuators  have  distinctive  func¬ 
tions.  The  drive  actuator.  A,  moves  the  foot  back 
and  forth  along  a  path  which  is,  to  a  good  approx¬ 
imation,  a  straight  line  parallel  to  the  body  axis. 
The  approximate  straight  line  is  generated  by  the 
four-bar  linkage  configuration  of  the  upper,  leg. 

The  shank  actuator,  B,  raises  and  lowers  the  foot 
at  the  beginning  and  end  of  the  contact  phase  and 
accommodates  to  variations  in  working  height  due 
to  stepping  in  depressions  or  on  bumps.  The  ab¬ 
duction-adduction  actuator,  C,  allows  the  leg  to 
be  tilted  outward  or  inward  to  perform  turning 
maneuvers  and  to  accommodate  to  side  slope  condi¬ 
tions. 

In  order  to  eliminate  valve  pressure  drop 
losses  and  to  allow  regeneration  it  would  be 
attractive  to  use  hydrostatic  power  transmission 
circuits.  However,  the  weight  and  cost  of  pro¬ 
viding  a  separate  variable  displacement  pusq>  for 
each  of  18  actuators  is  prohibitive.  Even  a  par¬ 
tial  system  using  separate  hydrostatic  circuits 
only  for  the  drive  actuators  is  unattractive. 
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Thus,  regeneration  using  conventional  types  of  hy¬ 
draulic  circuits  does  not  appear  possible.  How¬ 
ever,  the  load  and  displaceaent  rates  of  the  drive 
actuators  are  related.  This  feature,  coupled  with 
the  predictive  and  coordinating  power  of  the  con¬ 
trol  coaputer  peraits  design  of  a  single  circuit 
powering  all  6  drive  actuators  which  will  peralt 
regeneration.  Likewise,  if  all  6  shank  actuators 
are  grouped  into  a  single  circuit  and  all  6  ab¬ 
duction-adduction  actuators  likewise,  advantage 
can  be  taken  of  the  siailarities  of  load  and  dis- 
placenent  histories  of  the  actuators  in  each 
circuit . 

The  drive  circuit  has  load -displaceaent  char¬ 
acteristics  quite  slailar  to  that  of  a  wheeled 
vehicle.  Displaceaent  rate  is  large  and  varies 
only  slowly.  Load  varies  widely  but  not  rapidly. 
Restriction  of  vehicle  operation  to  wave  gaits 
(14,15)  and  to  those  in  which  legs  are  raised  and 
lowered  at  precisely  the  saae  instant,  that  is, 
those  with  duty  factors  of  5/6,  2/3,  or  1/2,  main¬ 
tains  constant  circuit  voluae.  Balanced  area  ac¬ 
tuators  will  be  used  for  the  saae  reason.  Aver¬ 
age  power  in  this  circuit  is  high  because  of  the 
large  displacements. 

The  return  portion  of  the  leg  cycle  presents 
a  problem.  As  far  as  possible  one  would  like  to 
convert  the  kinetic  energy  of  the  leg  at  the  end 
of  the  contact  phase  into  gravitational  potential 
energy  and  re-use  it  to  drive  the  leg  forward 
during  the  return  cycle.  This  requires  provision 
for  the  leg  to  swing  forward  freely  and  for  the 
delivery  to  it  of  a  relatively  small  amount  of 
energy  to  make  up  for  frictional  losses.  This 
type  of  action  is  quite  incompatible  with  the 
other  functions  of  the  drive  circuit.  Loading  on 
the  return  phase  is  low  compared  to  that  of  the 
contact  phase  and  average  joint  rates,  while  the 
same  as  for  the  contact  phase  in  the  tripod  gait, 
(duty  factor  1/2)  increase  to  twice  the  contact 
phase  rate  for  2/3  duty  factor  and  five  times  it 
for  5/6  duty  factor.  Free  swing  of  the  leg  can 
be  allowed  by  providing  a  large  diameter  bypass 
across  the  actuator  and  simultaneously  isolating 
the  returning  leg  from  the  drive  circuit  and 
opening  the  bypass.  The  problem  of  delivering 
make  up  energy  can  best  be  handled  in  the  way  that 
humans  and  animals  handle  it  by  rapidly  lifting 
the  leg  at  the  beginning  of  the  return  cycle  higher 
than  necessary  for  ground  clearance.  This  increas¬ 
es  the  potential  energy  available  to  drive  the  re¬ 
turn  cycle  provided  the  leg  la  allowed  to  drop 
back  to  grotmd  clearance  height  under  its  own 
weight  after  passing  the  maximum  velocity  position. 
Thus,  the  energy  pulse  required  is  effectively 
delivered  by  the  shank  actuator  circuit,  which  is 
much  better  suited  for  this  purpose  than  the  drive 
circuit. 

It  is  proposed  to  connect  the  drive  actuators 
of  legs  on  each  side  of  the  vehicle  in  series. 

The  optimum  supply  arrangement  is  a  separate  pump 
for  each  of  these  series  circuits.  A  single  pump 
could  be  used,  but  only  at  the  cost  of  Increased 
losses  during  turning  movements.  Control  of  the 
displacements  of  these  pumps  provides  both  the 


primary  speed  control  and  the  primary  steering  con¬ 
trol.  Net  flow  rates  in  each  of  the  circuits  are 
to  be  sensed  and  their  average  used  as  a  speed 
feedback  signal  and  their  difference  as  a  direc¬ 
tional  feedback  signal.  Thus,  the  "drive  circuit" 
becomes  two  circuits  with  interconnected  controls. 

The  series  connection  of  the  drive  actuators 
on  each  side  of  the  machine,  in  principle,  con¬ 
strains  the  rates  of  the  drive  actuators  of  all 
legs  on  that  side  of  the  vehicle  to  be  the  same. 

In  practice,  bleed  circuits  would  be  used  to  by¬ 
pass  some  flow  around  the  actuators  permitting 
speed  variation  as  required  during  turning  move¬ 
ments  and  in  high  duty  factor  gaits.  The  bleed 
circuits  also  permit  actuator  velocities  to  vary 
during  the  contact  phase.  This  mode  of  operation 
automatically  handles  the  problem  of  distributing 
the  drive  load  among  the  legs.  If  a  leg  starts  to 
slip,  its  share  of  the  load  will  automatically 
decrease  since  the  pressure  across  its  actuator 
will  drop  throwing  a  correspondingly  larger  share 
of  the  supply  pressure  across  the  actuators  of 
the  other  legs.  The  drive  circuits  are  shown  on 
Figure  7. 


Figure  7;  Drive  circuit  configuration.  The  heavy 
weight  lines  indicate  hydraulic  compo¬ 
nents  and  connections.  The  lighter 
weight  lines  indicate  control  informa¬ 
tion  paths.  Note  that  relief  valve  and 
charge  pump  circuits  are  not  included 
on  this  diagram. 


Since  flow  rate  is  relatively  difficult  to 
measure  directly,  it  is  probably  best  to  measure 
actuator  rates  and  infer  flow  rates  from  these. 
During  each  cycle  segment  the  computer  might  select 
as  master  leg  the  leg  with  the  highest  rate  on 
each  side.  If  that  leg  is  operated  without  bleed, 
its  drive  actuator  velocity  obtained  from  the 
joint  sensors  gives  the  flow  rate  for  that  side  of 
the  vehicle. 

Each  of  the  drive  actuator  circuits  is  a 
constant  volume  circuit  in  which  flow  rate  is  con¬ 
trolled  by  pump  displacement.  Load  reversal  in 
the  actuators  can  be  used  to  drive  the  pump  as  a 
motor  and,  hence,  to  return  power  to  the  flywheel. 
Although  it  has  multiple  actuators  which  are 
switched  in  and  out  of  the  circuit,  its  character¬ 
istics  are  those  of  a  hydrostatic  circuit. 

Figure  7  is  a  schematic  representation  of  the 
hydraulic  flow  paths  and  control  information  paths 
for  this  subsystem.  The  hydraulic  elements  are 
indicated  oy  heavy  lines  and  the  control  informa¬ 
tion  paths  by  lighter  lines.  Each  balanced  area 
actuator  is  controlled  by  three  valves.  A  and  B 
are  three-position  four  way  Vo.ves  and  are  used 
primarily  as  flow  switches.  Variation  of  the  flow 
rate  through  the  actuator  is  controlled  by  the  two 
way  flow  control  valve  C.  During  the  contact 
phase,  flow  through  C  is  small  and  so,  although  it 
is  subject  to  the  full  actuator  pressure  drop,  the 
energy  consumed  in  the  valve  will  be  small.  Dur¬ 
ing  the  return  phase,  valve  C  is  fully  open  per¬ 
mitting  relatively  free  swing  of  the  leg.  Valve  A 
functions  to  switch  the  actuator  in  and  out  of  the 
flow  circuit.  It  has  two  functional  states.  In 
the  first,  the  supply  is  connected  to  one  side  of 
the  actuator  and  the  return  to  the  other.  In  the 
second,  the  supply  is  connected  directly  to  the 
return,  bypassing  the  actuator.  The  two  ends  of 
the  actuator  are  also  connected  providing  an  addi¬ 
tional  actuator  reverse  flow  path  for  the  return 
phase.  Valve  B  is  provided  to  allow  reversal  of 
the  direction  in  which  the  actuator  is  driven 
allowing  positive  drive  of  the  return  motion  when 
the  leg  is  iitpeded  and  for  sprinting  with  in¬ 
creased  leg  return  speed.  Valve  B  is  also  to  be 
used  to  hold  the  leg  in  position  at  the  end  of 
the  return  phase  in  order  to  synchronize  leg 
placement.  Both  A  and  B  are  conventional,  three 
position  four-way  control  valves,  although  A  is 
connected  in  an  unusual  flow  configuration.  Note 
that  the  flow  paths  are  symmetric  with  respect  to 
reversal  of  the  direction  of  flow.  This  feature 
provides  a  capability  for  driving  the  machine  in 
reverse . 

The  operational  requirements  of  the  shank 
actuator  circuit  are  quite  different  from  those  of 
the  drive  circuit.  During  the  contact  phase  the 
shank  actuator  must  either  hold  position  or  exe¬ 
cute  small  movements  against  high  load.  At  the 
beginning  of  the  return  cycle  it  must  lift  rapid¬ 
ly  against  the  relatively  small  shank  and  foot 
weight  load.  During  the  later  portion  of  the  re¬ 
turn  phase  and  at  the  end  of  that  phase  it  should 
allow  the  foot  to  descend  under  its  own  weight. 


Since  there  is  such  a  disparity  between  load 
during  the  contact  phase  and  during  the  remainder 
of  the  cycle,  operation  as  a  simple,  constant  sup¬ 
ply  pressure,  valve  controlled  servo  would  require 
acceptance  of  severe  valve  losses  during  leg  lift 
because  of  the  high  rate  and  low  load  relative  to 
supply  pressure  which  would  have  to  be  high  enough 
to  support  the  contact  load.  Control  using  pump 
displacement  would  probably  have  too  narrow  a  band¬ 
width  to  accommodate  an  adequately  fast  leg  lift. 
In  order  to  overcome  this  disparity  in  operating 
conditions  it  is  proposed  to  provide  energy  stor¬ 
age  by  means  of  accusntlators  placed  closed  to  the 
shank  actuators  (figure  8) .  These  can  be  used  to 


Figure  S;  Shank  circuit  configuration 


release  a  short  pulse  of  energy  to  power  the  leg 
lift  with  minimal  effect  on  the  much  more  slowly 
responding  supply  system.  Valve  energy  losses  are 
s^nimized  by  keeping  the  valve  wide  open  during 
the  energy  transfer.  The  accumulator  should  be 
sized  to  have  sufficient  stored  energy  for  maximum 
leg  lift.  It  is  charged  during  the  remainder  of 
the  swing  phase  «dille  the  actuator  is  isolated 
from  the  supply.  A  controlled  bypass  is  also  pro¬ 
vided  to  allow  controlled  descent  of  the  leg  under 
its  own  weight.  The  shank  actxiator  is  isolated 
from  the  supply  during  this  phase.  Note  that  in 
this  circuit  the  pump  servo  is  set  up  to  control 
supply  pressure  rather  than  flow  rate  as  in  the 


drive  circuit.  The  four  way  control  valve  is  a 
conventional,  critical  center  valve  used  in  a  con¬ 
ventional  manner.  The  bypass  valve,  B,  is  used  to 
drop  the  leg  under  its  own  weight  and  not  as  the 
main  control  element  as  in  the  drive  circuit.  A 
bypass  with  a  check  valve  which  is  also  switched 
by  valve  B  permits  the  shank  to  continue  to  rise 
freely  after  delivery  of  a  brief  pressure  pulse 
from  the  accumulator.  The  pump  displacement  will 
be  controlled  to  provide  a  supply  pressure  com¬ 
manded  by  the  control  computer.  This  will  allow 
supply  pressure  to  be  matched  to  weight  with  a 
relatively  small  valving  margin. 

The  operational  characteristics  of  the  abduction- 
adduction  circuit  are  quite  different  again.  In 
straight  ahead  walking  on  level  ground,  these  ac¬ 
tuators  simply  hold  position  against  minimal  loads. 
They  primarily  come  into  use  when  turning.  Except 
for  extremely  sharp  turns  or  crab-walking,  the 
displacements  are  small.  Loads  are  highly  varia¬ 
ble.  Because  of  the  relatively  small  displace-, 
ments  expected  in  most  operating  conditions,  the 
valve  losses  of  a  conventional  valve  controlled 
circuit  should  be  acceptable.  The  primary  control 
variable  of  this  circuit  will  be  joint  rate  al¬ 
though  position  will  also  be  sensed  and  controlled. 
The  computer  will  be  used  to  regulate  supply  pres¬ 
sure  in  accordance  with  expected  actuator  loading. 
The  circuit  is  shown  in  Figure  9.  Shot  pin  loca¬ 
tion  will  be  used  on  this  and  the  drive  circuit 
to  hold  position  when  powered  down.  The  shank 
will  be  allowed  to  rest  on  its  upper  stop  when 
powered  down,  effectively  kneeling  the  vehicle. 

CONCLUSIONS 

It  should  be  emphasized  that  the  design  of 
the  system  described  here  is  still  developing. 
Although  the  broad  outlines  of  the  power  supply, 
storage  and  transmission  system  have  been  resolved, 
many  questions  pertaining  to  the  details  of  design 
and  operation  reisain  to  be  answered.  Investiga¬ 
tions  are  in  progress  of  system  dynamic  response, 
switching  problems,  fluid  transients  and  dynamic 
optimization.  These  aspects  of  the  system  are 
being  studied  by  a  combination  of  computer 
simulation  and  of  testing  breadboarded  subsystems. 


Figure  9:  Abduct ion -Adduct ion  circuit  configura¬ 
tion 


3.  Kaiser  Spyder  Specifications,  Industrial  and 
Municipal  Engineering  Corporation,  Galva, 
Illinois. 
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ABSTRACT 


One  of  the  causes  of  low  efficiency  of  legged  vehicles  is  the  type  of  actua¬ 
tion  systea  used.  This  paper  describes  the  overall  design  and  the  evaluation  of 
soae  aspects  of  a  proposed  hydraulic  actuation  system  for  a  six-legged  vehicle 
intended  for  use  in  rough  terrain.  Features  of  the  hydraulic  actuation  system 
designed  to  improve  mechanical  efficiency  are  described,  A  combination  of  line¬ 
arized  dynamic  system  analysis  and  computer  simulation  of  the  nonlinear  dynamic 
system  equations  is  used  to  evaluate  some  aspects  of  the  proposed  design.  The 
tradeoff  between  energy  efficient  operation  and  the  dynamic  performance  of  the 
actuation  system  is  investigated.  Some  criteria  for  controller  design  are 
enumerated . 
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INTRODUCTION 


Legged  vehicles  have  considerable  potential  for  improved  mobility  over 
rough  terrain  as  compared  to  wheeled  or  tracked  vehicles  {!] ,  Some  commercial 
machines  using  a  combination  of  articulated  limbs  and  idieels  have  demonstrated 
improved  nobility  characteristics  over  such  terrain  [2,3].  A  number  of  experi¬ 
mental  walking  machines  have  also  been  built  [4,S]  and  have  demonstrated  that 
terrain  adaptive  features  can  be  included  in  the  designs  of  legged  vehicles. 

Though  legged  vehicles  should  be  more  energy  efficient  for  locomotion  over 
soft  terrain  than  wheeled  or  tracked  vehicles  16],  fully  terrain  tdsptive  legged 
vehicles  have  proved  much  less  efficient  in  practice.  Reasons  for  the  low  me¬ 
chanical  efficiency  of  terrain  adaptive  legged  vehicles  have  been  identified  by 
Waldron  and  Klntel  [7] .  These  vehicles  have  tended  to  use  actuator  placement 
geometries  which  result  in  a  significant  amount  of  "back  drive”  work  during  the 
walking  cycle.  An  actuator  is  said  to  be  "back  driven”  when  it  acts  as  a  brake. 
The  mechanical  design  of  a  leg  which  minimizes  the  amount  of  "back  drive"  work 
has  been  described  by  Song  et  al .  [8] .  Another  Important  effect  is  energy 
wastage  due  to  the  oscillatory  nature  of  the  leg  motion.  This  can  be  reduced  by 
operating  the  leg  as  a  compound  pendulum  [7] .  It  is  also  desirable  to  use  ener¬ 
gy  storage  and  regeneration  on  legged  vehicles  for  use  in  very  rough  terrain. 

Prime  mover  operating  conditions  can  then  be  optimized  and  energy  absorbed  when 
braking  can  be  partially  recovered. 

Yet  another  factor  in  the  low  mechanical  efficiency  of  some  walking  machines 
is  the  type  of  power  transmission  and  actuator  used.  The  General  Electric  Quad- 
niped  [3],  for  example,  used  a  large  number  of  valve  controlled  hydraulic  actua¬ 
tors.  Such  a  method  of  actuation  results  in  good  dynamic  response  of  the  actua¬ 
tion  system  but  low  mechanical  efficiency  due  to  high  energy  losses  at  the  con¬ 
trol  valves. 


This  paper  describes  the  overall  design  and  the  evaluation  of  some  aspects 
of  a  proposed  hydraulic  actuation  system  for  a  six-legged  vehicle  for  use  in 
rough  terrain.  Features  of  the  hydraulic  actuation  system  designed  to  improve 
mechanical  efficiency  are  described.  A  combination  of  linearized  dynamic  system 
analysis  and  co^wter  simulation  of  the  nonlinear  dynamic  system  equations  is 
used  to  evaluate  the  proposed  design.  The  tradeoff  between  energy  efficient 
operation  and  the  dynamic  performance  of  the  situation  system  is  investigated. 

As  a  result  of  the  study,  some  criteria  for  controller  design  are  enumerated. 
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HYDRAULIC  ACTUATION  SYSTEM  DESIOt 

Hydraulic  actiiatlon  was  chosen  for  the  slx^legged  vehicle,  with  an  estlaated 
weight  of  1361  Kg  (3000  lb] ,  because  of  the  higher  force.to»welght  ratio  of  a  hy¬ 
draulic  system.  Hj^raulic  actuation  is,  aoreover,  ce^atible  with  the  low  speed 
high  load  nature  of  the  application.  Regeneration  of  energy  absorbed  when  brak¬ 
ing  is  also  possible  with  a  hydraulic  systea  when  used  In  conjunction  with  an 
energy  storage  systea. 

Figure  1  shows  a  conceptual  drawing  of  the  vehicle  leg.  Each  leg  of  the  aa- 
chine  is  to  have  three  degrees  of  freedoa  each  powered  by  a  linear  actuator.  The 
three  actuators  have  distinct  functions.  The  drive  actuator  A  aoves  the  foot 
back  and  forth  along  a  path  which  is,  to  a  good  approx last ion,  a  straight  line 
parallel  to  the  body  axis.  The  approxlaate  straight  line  is  generated  by  the 
four-bar  linkage  configuration  of  the  upper  leg.  Such  a  design  alnialtes  the 
"back  drive"  work  in  walking  by  decoupling  vertical  and  lioritontal  foot  aovaaents 
and,  hence,  iaproves  mechanical  efficiency  [8] .  The  shank  actuator  B  raises  and 
lowers  the  foot  at  the  beginning  and  end  of  the  contact  phase  and  acconaodates  to 
variations  in  working  height  due  to  stepping  in  depressions  or  on  bumps.  The 
abduction-adduction  actuator  C  allows  the  leg  to  be  tilted  outward  or  inward  to 
per fora  turning  aaneuvers  and  to  accoaaodate  to  side  slope  conditions. 


Fig.  1  Conceptual  Drawing  of  Vehicle  Leg 


The  design  of  the  hydraulic  actuation  systea  takes  advantage  of  the  special 
nature  of  the  acttutor  load  and  velocity  re^ireaents  for  legged  vehicles  using 
linkage  legs  of  the  type  shown  in  Figure  1.  The  drive  actuator  provides  aost  of 
the  power  in  straight  walking.  Drive  actuator  load  and  velocity  requirements  are 
high  during  the  contact  phase  of  the  walking  cycle.  Figure  2  shows  drive  actua¬ 
tor  load  requireaent  as  a  function  of  time,  for  a  walking  speed  of  4.0  Xa/hr 
(2.S  aph)  over  terrain  with  a  S0\  gradient,  for  a  1361  Kg  (3000  lb)  vehicle  with 
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a  specific  leg  geoaetry.  Figure  3  shows  the  drive  actuator  velocity  requireaents 
as  a  function  of  tiae  for  the  saae  leg  geometry.  A  tripod  walking  gait  is 
assuaed  [9],  In  such  a  gait,  at  any  one  instant,  the  comer  legs  on  one  side  of 
the  vehicle  end  the  center  leg  on  the  other  side  of  the  vehicle  are  in  contact 
with  the  ground.  The  remaining  legs  are  in  the  return  phase  of  the  walking  cycle. 
Though  velocity  requirements  are  Identical  for  the  drive  actuators  when  walking 
in  a  straight  line  in  tripod  gait,  the  load  requireaents  are  not.  The  drive 
actuators  on  the  comer  legs  in  contact  with  the  ground  share  half  the  vehicle 
load  whereas  the  drive  actuator  on  the  center  leg  on  the  other  side  supports  half 
the  vehicle  load.  The  drive  actuator  loads  vary  with  the  terrain  gradient  in  a 
predictable  msmner.  They  are  very  low  on  level  terrain  since  only  frictional  re¬ 
sistance  is  to  be  overcome.  They  increase  with  gradient  when  going  uphill. 
Corresponding  amounts  of  energy  must  be  absorbed  via  the  drive  circuit  when  going 
downhill . 
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Fig. 2  Drive  Actuator  Load  Versus  Time-4.0  Km/hr  (2,5  mph]  Nalking  Speed, 
50%  Terrain  Gradient,  1361  Kg  (3000  lb)  Vehicle  Weight. 


Figure  4  is  a  schematic  representation  of  one  drive  circuit  design  concept.  The 
hydraulic  connections  are  indicated  by  continuous  lines  and  the  control  paths  by 
dashed  lines.  The  six  drive  actuators  are  grouped  into  a  single  hydraulic  cir¬ 
cuit  powered  by  a  variable  delivery  pu^.  The  pressure  difference  across  the 
pump  is  the  supply  pressure  for  the  drive  actuators  and  is  controlled  by  a  feed¬ 
back  loop.  Each  of  the  drive  actuators  is  controlled  by  a  four-way  electrohy- 
draulic  servovalve.  The  valve  actuator  co^ination  forms  part  of  a  feedback  con¬ 
trol  system  in  which  actuator  velocity  is  the  controlled  variable. 

The  desired  supply  pressure  is  determined  by  the  control  computer  as  a  func¬ 
tion  of  the  mean  terrain  slope  and  mean  vehicle  speed.  In  order  to  reduce  energy 
losses  at  the  valves,  the  supply  pressure  is  chosen  so  as  to  keep  the  valves  near¬ 
ly  wide  open.  The  pressure  drop  across  the  valve,  for  a  given  flow  rate,  is 
thus  reduced. 

Each  balanced  area  actuator  is  controlled  by  four  valves.  Valve  A  is  an 
open  center,  three-position  valve  used  as  a  flow  switch.  During  the  contact 
phase,  it  is  actuated  as  shown  on  actuators  1,  4  and  5  in  Figure  4.  During  the 
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DTiv«  Actuator  Velocity  Versus  Tine  •  4.0  Kn/hr  (2.5  noh)  Nalking  Speed 


Fig.  4.  Schenatic  Representation  of  Drive  Circuit  Concept. 


return  phase,  valve  A  is  actuated  in  the  other  direction  as  shown  on  actuators  2, 
3  and  6  in  the  figure  and  isolates  the  actuators  froa  the  drive  circuit.  Valve  B 
is  a  continuously  variable,  electrohydraulic  four-way  servovalve.  Ifith  the  two- 
position  valve  C  actuated  as  shown  for  actuator  4,  valve  B  controls  the  actuator 
notion  in  a  conventional  manner.  Actuation  of  valve  C  as  shown  for  actuators  1 
and  5  connects  the  actuators  in  series.  Valve  D  is  actuated  differently  during 
the  contact  and  return  phases  of  the  walking  cycle  as  shown  in  Figure  4. 


In  tripod  gait,  the  drive  actuators  on  the  comer  legs  in  contact  with  the 
ground  are  connected  in  series.  The  series  connection  handles  the  problem  of 
distributing  the  drive  load  amoi.g  the  comer  legs  as  shown  in  Figure  2.  The 
drive  actuator  on  the  center  leg  on  the  other  side  of  the  vehicle  is  also  in  con¬ 
tact  with  the  ground  and  is  controlled  by  the  four-way  servovalve  B  in  a  conven¬ 
tional  manner.  The  other  three  actuators  are  on  returning  legs  and  are  isolated 
from  the  drive  circuit.  Figure  4  shows  the  hydraulic  circuit  operation  for  tri¬ 
pod  gait. 

The  return  phase  of  the  walking  cycle  requires  a  completely  different  node 
of  control  as  described  by  Waldron  et  al  [10]  if  excessive  energy  losses  are  to 
be  avoided.  The  drive  actuators  on  the  returning  legs  are  isolated  from  the 
drive  circuit  and  allowed  to  swing  back  freely  by  opening  a  large  diameter  bypass 
across  each  such  actuator.  The  shank  circuit,  used  to  power  the  shank  actuators, 
is  also  capable  of  providing  stake  up  energy  to  drive  actuators  on  returning  legs 
[10].  The  control  valves  in  the  drive  circuit  allow  implementation  of  the  de¬ 
sired  control  action  during  the  return  phase. 

Turning  maneuvers  are  accosssodated  naturally  by  the  drive  circuit.  The 
drive  actuators  on  the  inside  of  the  turn  curve  would  be  operated  at  a  lower 
speed  than  those  on  the  outside  of  the  turn  curve  in  response  to  coordinated 
command  inputs  to  the  velocity  servomechanisms. 

While  walking  downhill  at  constant  speed,  the  drive  actuators  experience  a 
pressure  rise  instead  of  a  pressure  drop.  The  desired  supply  pressure  is  set  by 
the  control  computer  to  be  a  negative  value.  The  variable  delivery  pump  is  driv¬ 
en  as  a  motor,  in  effect,  and  the  recovered  energy  can  be  stored  if  there  is 
provision  to  do  so. 

The  design  features  and  the  dynamic  response  of  drive  circuit  components  are 
of  primary  interest  in  this  paper.  The  operational  requirements  and  the  designs 
of  the  shank  circuit  and  the  abduction-adduction  circuit  are  described  elsewhere 
by  Waldron  et  al  [10] . 

DYNAMIC  ANALYSIS  OF  DRIVE  CIRCUIT  COMPONENTS 

The  design  of  the  drive  circuit  and  its  proposed  manner  of  operation  have 
been  governed  by  the  need  to  reduce  energy  losses  and  improve  overall  efficiency 
of  operation.  The  resulting  design  features  and  operational  characteristics 
affect  the  dynamic  perforpance  of  the  actuation  system  significantly.  In  this 
section,  two  components  of  the  drive  circuit  are  investigated.  Firstly,  the  dy¬ 
namic  performance  of  electrohydraulic  velocity  servomechanisms  operated  with 
nearly  wide  open  servovalves  is  examined.  Secondly,  the  dynamic  performance  of 
servomechanisms  employing  drive  actuators  connected  in  series  is  considered.  The 
nonlinear  equations  describing  system  behaviour  are  simplified  to  enable  linear 
analysis  of  the  dynamic  performance.  The  nonlinear  equations  are  used  subsequent¬ 
ly  to  simulate  the  dynamic  behaviour  of  the  drive  circuit  coaqionents  on  a  digital 
cosqniter. 

Dynamic  Analysis  of  a  Balanced-Area  Actuator  Controlled  by  a  Four-Way  Servovalve 

Consider  the  single  balanced  area  actuator  controlled  by  a  ^our-way  servo- 
valve  as  shown  in  Figure  5.  The  valve-actuator  combination  forms  part  of  the  ve¬ 
locity  servomechanism  shown  in  Figure  6.  The  equations  describing  the  dynamic 
behaviour  of  the  system  are  given  below  and  can  be  found  in  most  standard  text¬ 
books  on  electrohydraulic  servomechanisms  [11].  The  symbols  are  defined  in  the 
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Noaenclature  saction  of  the  paper. 


The  valve  flow  equations  for  a  critical  center  servovalve  are  given  below 
assuaing  the  valve  is  to  be  open  in  the  direction  shown  in  Figure  5. 

QiCt)  -  w  x^j(t)  \/|  C 


QjCt)  .  Cj  N  x^j(t)  >/  f  (Pi2(t)-P,) 

The  continuity  equations  for  the  two  sides  of  the  actuator  are 
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The  volunes  V^^(t).  ^  actuator  chafers  are 
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Vj2Ct)  •  -  Vjj(t)  (6) 

The  load  equation  for  the  actuator  piston  is  given  by 

dx  ,(t) 

*p('’ii(*5-’’i2(*^^ "  ^l(*^  *  ®p 

where  Fi,x(t)  is  the  conputed  load  on  the  drive  actuator  corresponding  to  a 
noninal  vehicle  notion  trajectory  and  dF(t),  the  excess  force  on  the  drive 
actuator,  results  in  deviations  fron  the  noninal  trajectory. 


Fig.  S.  Valve-Actuator  Conblnation  for  Canter  Leg. 


26 


VcJocitr 

7rM»^«r 


Fig.  6.  Leg  Velocity  ServoMchanin 


Appendix  A  details  the  derivation  of  the  relationship  between  the  excess 
force  dP(t)  and  the  velocity  and  acceleration  of  the  actuator  and  the  vehicle 
center  of  nass.  The  resulting  equation  indicates  that  hF  is  a  nonlinear  and 
tine  varying  function  of  actuator  position,  velocity  and  acceleration. 


AF(t) 


-  *, ) 


(8) 


Vo>  Aq,  Vg,  ag  are  coaputed  noainal  vehicle  velocity  and  acceleration  and  corre* 
spending  actuator  velocity  and  acceleration  respectively.  They  are  all  functions 
of  linkage  or  actuator  position.  M  is  a  fraction  of  the  vehicle  nass  and  corre¬ 
sponds  to  the  fraction  of  the  vehicle  weight  supported  by  the  leg.  It  is  a  tine 
varying  function  and  depends  on  the  positions  of  all  the  legs  in  contact  with  the 
ground.  For  the  actuators  on  the  center  legs  in  tripod  gait,  M  is  one-half  the 
vehicle  nass. 


The  equations  listed  above  are  adequate  to  define  the  actuator  notion  ]^i(t} 
in  terns  of  the  servovalve  spool  displacenent  Xvi(t)  and  the  actuator  load  Ffj(t) 
if  the  noninal  vehicle  and  actuator  velocities  and  accelerations  and  the  vehicle 
nass  fraction  M  are  known.  For  purposes  of  a  linear  analysis,  the  equations  need 
to  be  sinplified  considerably.  Appendix  A  indicates  that  the  sinplest  linearized 
form  of  equation  (8)  is 

MV  ^ 

^F(t)  . 

(9) 


where  M(  is  the  average  reflected  inertia  at  the  actuator  corresponding  to  the 
vehicle  nass.  The  renaining  dynanic  equations  can  be  linearized  in  a  standard 
nanner  for  snail  perturbations  about  a  nean  operating  condition,  resulting  in  a 
transfer  function  relationship  between  Xpi,  Xyi  and  F|.x  [11]. 
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The  servovalve  dynanics  are  represented  by  a  first  order  aodel  [12]  as  shown  in 
Figure  6. 

Energy  efficient  operation  of  the  servonechanis*  requires  that  Pj  be  adjust* 
ed  for  different  operating  conditions  to  keep  the  pressure  drop  across  the  servo¬ 
valve  low  for  a  given  flow  rate.  As  a  consequence,  the  value  of  the  flow  gain  IL 
is  low.  This  fact,  in  itself,  is  not  a  disadvantage  if  the  controller  design  cari 
be  altered,  as  nean  operating  conditions  change,  to  aaintain  a  high  loop  gain. 
However,  the  gains  K.  and  K.  are  aore  sensitive  to  changes  in  the  supply  pressure 
Pj,  the  aean  load  pressure  (Pn*^12)a  ***’’  spool  dlsplaceaent  (Xy;)^  as 

a  result  of  the  low  servovalve  pressure  drops. 


2.PCP,-(Pii-Pi2),) 


(14) 
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Therefore,  paraaeters  in  the  linear  systea  represented  by  equations  (10)-(13)  and 
in  the  controlled  systea  of  Figure  6  change  significantly  during  the  contact 
phase  of  the  walking  cycle.  Therefore,  one  objective  of  controller  design  based 
on  the  linear  aodel  of  the  systea  should  be  to  reduce  the  sensitivity  of  the 
controlled  systea  perfoiaance  to  changes  in  the  paraaeters  and  K^. 

Another  consequence  of  nearly  wide  open  servovalve  operation  during  the  con¬ 
tact  phase  is  a  high  value  of  the  flow-pressure  coefficient  Kce-  Higher  Kce  re¬ 
sults  in  a  lower  stiffness  of  the  velocity  servoaechanisa  to  the  actuator  load 
Fi,i .  Since  the  actuator  load  Fu  is  expected  to  be  high  compared  to  the  inertial 
load  tF  for  a  legged  vehicle,  another  objective  of  controller  design  should  be  to 
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laprove  the  servoaechania  stiffnete. 


SoM  coapllace  is  desirable  in  vehicles  used  for  locoMCion  over  rough 
terrain.  The  achievaent  of  active  coupliace  for  a  legged  vehicle,  by  appropri¬ 
ate  foraulation  of  the  supervisory  control  algoritha,  has  been  describe  by  Klein 
wd  briggs  [13].  It  is  desirable  to  aodify  the  mpervisory  control  algoritha  to 
utilize  the  velocity  servaechania  conpliMCe  to  the  extent  possible. 

Let  us  consider  the  following  parsMter  values  for  the  servonechania 
coaponents  for  the  purpose  of  conducting  a  linear  aalysis. 

Servovalve  Actuator 

b  •  1.S4  ca  (1.0  in)  A  •  16.1  a*  (2.5  in*) 

C  -  0.61  V?  -  4P2  cm*  (30  in‘) 

(X  .)  «  0.0770  ca  (0.0303  in)  r  .  r  -  a  in*/se<^ 

‘^ep  *  °  ^50 

T  ■  0.00634  sec 


B 

P 

The  paraeters  correspond  to  a  56.7  Ipa  (IS  gpa)  two-stage  electrohydralic 
servovalve.  The  drive  actuator  is  sized  to  provide  adequate  force  and  flow  rate 
capability  for  a  1361  Kg  (3000-lb)  six  legged  vehicle  using  the  leg  design  re¬ 
ferred  to  in  Figures  2,3  to  operate  at  speeds  of  up  to  8.0  Xa/hr  (5  aph)  ad  over 
terrain  with  up  to  a  50%  gradient,  if  a  aaxiaua  supply  pressure  of  2069  K/ca* 
(3000  psi)  is  available.  The  leg  design  being  considered  also  results  in  a  leg 
cycle  tine  of  approxiately  0.93  seconds  for  a  tripod  gait  at  8.0  Ka/hr  (5  aph). 
The  reference  input  to  the  velocity  servoaechania  is,  therefore,  a  perl^ic  in¬ 
put  with  the  sae  period  as  the  leg  cycle  tiae. 

If  a  fixed  controller  is  used  in  the  servoaechaia  of  Figure  6,  the  systa 
dynaic  response  and  the  operating  efficiacy  would  chage  with  the  aa  operat¬ 
ing  conditions.  The  vaiation  in  the  systa  dynaic  response  is  reduced  if  the 
supply  pressure  is  aintalned  constat  at  a  value  of  one  and  one-half  tiaes  the 
axiaua  expected  load  pressure,  but  the  systa  efficiacy  variation  is  grater 
[II].  If  the  supply  pressure  is  adjusted  to  atch  the  ma  opaating  conditions, 
the  efficiency  variatia  is  reduced  but  the  systa  dynaic  response  vales 
considerably . 

Table  1  indicates  the  closed  velocity  loop  perfoiaance  for  a  proportional 
catroller  with  gain  K«,  at  diffaat  operating  conditions.  For  euh  operating 
condition,  the  aa  load  pressure  at  the  atuator  and  the  ua  actator  velocity 
are  also  tabulated.  The  controller  gain  K  is  selected  for  eah  operating  condi¬ 
tion  to  give  a  60*  phase  argin  based  on  the  linearized  servoaechalM  aodel  for 
that  operating  cmdition.  The  results  in  Table  1  indicate  that  variation  in  the 
systa  dynaic  perfonaee  for  a  wide  rage  of  operating  conditlms  is  reduced 
by  controller  adjustaat.  Also,  for  a  specified  operating  condition,  the  systa 
dynaic  perfoiaance  ca  be  aintalned  at  about  the  sae  level  eva  if  the  supply 
pressure  is  reduced  to  iaprove  the  systa  operating  efficiacy.  For  inatace, 
supply  presare  reduction  fra  1655  N/a*  (2400  psi)  to  1104  N/a*  (1600  psi)  for 
a  4.0  K^hr  (2.5  aph)  alXlng  speed  ova  a  50%  gradiat  iaproves  the  aoa  operat¬ 
ing  efficiency  fra  56%  to  84%  while  catroller  adjustaat  aintains  the  saa 
dynaalc  perforaace.  The  closed  loop  bandwidth  and  the  stady  state  coapliance 
achievable  ala  sea  appropriate  for  the  application.  They  shald  remit  in  the 
wtal  leg  velaity  tracking  the  desired  leg  velalty  rasoably  mil,  for  the 
rage  of  walking  speeds  ad  leg  loads  expected. 
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fsble  1.  Center  Leg  Velocity  ServoMchaniea  Performance  Based  on  a  Linear 
Model. 


A  proportional  controller  is  the  simplest  type  of  controller  that  can  be 
used.  Alternative  controllers  can  be  designed  to  improve  some  aspects  of  the 
system  dynamic  performance  at  the  expense  of  others.  The  closed  velocity  loop 
performance  given  in  Table  1  indicates,  however,  that  a  generally  satisfactory 
level  of  performance  can  be  achieved  even  with  proportional  control,  if  the  con¬ 
troller  gain  is  adjusted  as  mean  operating  conditions  change.  The  details  of 
the  controller  gain  adaptation  would  need  to  be  investigated  further  to  ensure 
satisfactory  transient  response  of  the  controlled  system  as  the  operating  condi¬ 
tions  change. 

As  a  first  step  towards  a  better  evaluation  of  the  closed  loop  system  per¬ 
formance,  its  dynamic  behaviour  during  the  contact  phase  is  simulated  using  the 
Continuous  Systems  Modeling  Program,  CSMP  [lA] .  The  nonlinear  dynamic  equations 
(lj-(8)  are  simulated  for  some  of  the  operating  conditions  noted  in  Table  1. 

The  load  force  FLi(t)  on  the  drive  actuator,  the  share  of  vehicle  mass  assigned 
to  the  leg  M,  the  mean  vehicle  velocity  Vq,  the  desired  actuator  velocity  v©  and 
acceleration  ao  are  computed  as  functions  of  time  corresponding  to  a  constant 
walking  speed  in  tripod  gait  and  are  used  to  characterize  the  actuator  loading  in 
equations  (7)  and  (8) .  Because  of  the  manner  of  computation  of  M,  the  simulation 
is  valid  only  for  small  variations  of  the  actuator  trajectory  about  the  nominal 
trajectory  corresponding  to  the  constant  speed  tripod  gait  assumed.  A  propor¬ 
tional  controller  with  gain  Ka  and  a  first  order  representation  of  servovalve 
dynamics  are  assumed  as  in  Figure  6.  The  servovalve  and  actuator  parameters  and 
the  fluid  properties  noted  in  equation  (17)  are  used  in  the  simulation.  Limits 
on  the  servovalve  spool  displacement  and  upper  and  lower  pressure  limits  of  Ps 
and  0  are  also  included  in  the  simulation.  The  dynamics  of  the  supply  pressure 
control  loop  are  neglected  and  Py  is  assumed  to  be  constant  for  each  simulation 
run. 


Figure  7  shows  the  simulated  response  of  the  velocity  servemechanism  during 
the  contact  phase  of  the  walking  cycle.  The  velocity  error  is  low  except  idien 
the  leg  contacts  the  ground.  The  stiffness  of  the  servomechanism  is  acceptable 
but  is  limited  by  the  supply  pressure  of  1104  N/cm*  (1600  psi),  as  Indicated  by  the 
wide  open  servovalve  during  the  initial  part  of  the  contact  phase.  The  average 
efficiency  of  the  valve  actuator  combination  during  the  contact  phase  is  approx¬ 
imately  84%.  It  should  also  be  noted  that  the  proportional  controller  gain  used 
for  Figure  7  is  a  factor  of  1.5  times  the  gain  listed  in  Table  1  for  the  same 
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opwrktlng  conditions.  Other  siauletlon  runs  also  indicated  that  controller  gains 
higher  than  those  suggested  by  the  linear  analysis  can  be  used  in  the  slaulation 
without  degrading  systea  stability. 
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Fig.  7.  Siaulated  Response  of  the  Velocity  Servonechanisn  -  P*  •  1104  N/c»* 
(1600  psi),  V  •  4.0  ICn/hr  (2.S  nph),  K-K-Kt  •  0.162  sec,  S0\ 
Terrain  GradlSnt. 


The  slaulation  runs  noted  In  Figure  8  indicate  that  average  efficiencies  of 
about  80%  can  be  achieved  together  with  good  dynaaic  response  at  aoderate  and 
low  speeds  and  at  aoderate  and  high  load  levels.  Efficiencies  at  high  speeds 
and  low  loads  are  United  by  the  relatively  large  valve  pressure  drops  at  high 
flow  rates  and  by  the  fact  that  servovalve  supply  pressures  for  two-stage  servo¬ 
valves  have  to  be  naintalned  higher  than  a  threshold  level  for  proper  operation, 
even  for  low  load  levels.  Another  liaitation  on  reducing  supply  pressure  is  that 
the  servovalve  dynaaic  response  degrades  as  the  supply  pressure  decreases.  There 
are  definite  efficiency  gains  to  be  achieved,  however,  by  oversizing  servovalves. 
For  instance,  if  a  113  ipn  (30  gpa)  servovalve  were  used  for  the  present  applica¬ 
tion,  the  average  efficiency  for  a  8.0  ka/hr  (5  aph) walking  speed,  18%  terrain 
gradient  and  S17  N/ca*  (750  psi)  supply  pressure  would  be  70%  Instead  of  the  44% 
noted  for  curve  (c)  in  Figure  8. 

The  analysis  and  slaulation  results  indicate  that  the  linear  aodel  of  servo- 
aechanisa  dynaaics  represented  by  equations  (8) -(9)  and  Figure  6  leads  to  the  se¬ 
lection  of  lower  controller  gains  than  those  suggested  by  the  slaulation.  One 
possible  refineaent  in  the  aodel  is  a  describing  function  description  of  the  kine- 
aatics  of  the  linkage  leg  [IS].  The  resulting  aodel  would  enable  a  bettor  evalua¬ 
tion  of  proposed  controller  designs  than  the  linear  aodel  and  would  have  reduced 
coaputatlonal  requireaents  as  coapared  to  the  coaputer  slaulation  presently  used. 

The  results  cited  also  indicate  the  iaportance  of  controller  adjustaent  as 
aean  operating  conditions  change,  to  help  achieve  good  servoaechanisa  dynaaic  re¬ 
sponse  and  operational  efficiency.  The  nonlinear  and  tiae  varying  nature  of  the 
servoaechanisa  dynaaics  suggests  that  controller  designs  requiring  an  accurate 
aodel  of  the  systea  dynaaics  would  not  be  feasible.  Since  controller  adaptation 
is  considered  desirable,  a  aodel  refermce  adaptive  control  scheae  aay  provide 
the  necessary  fraaework  for  controller  adaptation.  Such  a  controller  has  the 
additional  advantage  of  not  requiring  a  coapleta  aodel  of  the  system  dynaaics  [16]. 
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Fig.  8.  Efficiencies  During  the  Contact  Phase  for  Different  Operating 
Conditions. 


Dynaaic  Analysis  of  Balanced  -  Area  Actuators  Connected  in  Series  and  Controlled 
by  Four-Way  Servovalves 

Figure  9  shows  two  balanced*area  actuators  connected  in  series  and  controlled 
by  servovalves.  The  dynaaic  equations  describing  systea  behaviour  are  straight¬ 
forward  extensions  of  equations  (l)-(8)  and  are  not  listed  here.  The  pressures 
PX2  and  P21  in  the  actuator  chaabers  connected  to  each  other  are  assuaed  to  be 
the  saae.  The  series  connection  of  the  actuators  couples  their  dynaaic  behaviour. 
The  coupled  differential  equations  can  be  linearixed  as  for  the  single  cylinder 
case,  are  aalce  the  additional  ssstaption  that  the  actuator  internal  and  external 
leakage  coefficients  Cip  and  C«p  are  negligible  since  the  mean  servovalve  spool 
displaceaents  (xyi^a  (Xv2)a  are  high  during  the  contact  phase  and  result  in 
large  values  of  the  valve  flow-pressure  coefficients  K^i  and 


nil _ 


1 1 

* 

Fig.  9.  Valve-Actuator  Configuration  for  Comer  Legs. 


The  resulting  coupled  fourth-order  differential  equations  are  given  below, 
assuaing  the  servovalves  to  be  open  in  the  directions  shown  in  Figure  8. 

Ai(s)Xpi(s)  ♦  B^CsJXpjCs)  -  Cj(s)x^j(s)  ♦  DjCi)  Fj^j(s)  (17) 

AjCsJXpiCs)  ♦  BjCs)Xp2(»)  •  CjCslXyjCs)  ♦  DjCs)  Fl2C») 

where 
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The  linearized  valve  coefficients  K  ,  K  ,  K  ,  K  in  the  equations  above 


are  defined  by 
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N^i,  M(2  4^^  average  reflected  inertias  at  the  two  actuators  corresponding  to  the 
venic  ‘ 


liicle  Mass. 


The  dynaaics  of  the  valve-actuator  coabinations  are  therefore  represented  by 
a  aultivariable  systea  with  two  control  inputs  JiyiCt)  and  Xy2(t},  two  disturbance 
inputs  Fi_|(t)  and  FL2(i)  outputs  apx(t)  and  Xp2(t).  p  ^ 


«pl(s) 

Gi/(») 

^2**^*^  *'li**^*^  '*12'*^*^ 

“p2^*i 
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(28) 


The  elements  of  the  transfer  function  aatrix  are  defined  in  terns  of  the  polyno¬ 
mials  in  equations  (19)-(26). 


The  same  parameter  values  are  used  as  in  the  single  actuator  case  with  the 
exception  of  the  reflected  inertia  and  the  servovalve  time  constant  t.  Mti 
and  M{2  are  each  assumed  to  be  6924  N-sec/m^ (474.7  slugs)  and  the  servovalve 
dynaaics  are  considered  to  be  negligible.  The  servovalves  in  the  two  servo- 
mechaniSBs  are  assumed  to  operate  identically  at  steady  state  resulting  in  the 
linearized  valve  coefficients  in  equation  (27)  being  the  saae  for  the  two  servo¬ 
valves.  The  coupled  servomechanisms,  therefore,  have  identical  dynaaic  charac¬ 
teristics. 


Table  2  indicates  the  closed  velocity  loop  performance  based  on  the  linear 
model,  for  proportional  controllers  with  identical  gains  K,  for  the  two  servo¬ 
mechanisms.  Kj  is  selected  to  be  the  saae  as  for  the  single  actuator  case  at  the 
saae  operating  conditions.  The  resulting  values  of  steady  state  accuracy  and 
compliance  are  therefore  identical  to  those  in  Table  1  for  similar  operating 
conditions.  Table  2  indicates,  however,  that  the  3-db  closed  loop  bandwidths  are 
lower  for  the  coupled  servoaechanisas.  The  closed  loop  bandwidth  is  evaluated 
assuming  that  the  two  servoaechanisas  have  identical  reference  Inputs  v.^t  v^2- 
Coaputation  of  the  closed  loop  poles  indicated  that  the  systea  was  stable  in  the 
three  cases  examined.  The  degree  of  stability  of  the  coupled  servoaechanisas 
was  not  determined. 
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Table  2.  Comer  Leg  Velocity  Servoaechanisa  Perforaance  Based  on  a  Linear  Model. 
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A  aore  ntensiv*  investigation  of  tlie  dynaalc  behaviour  of  the  series- 
connected  actuators  is  called  for  and  Mill  be  undertaken  shortly.  It  is  expected 
however  that,  as  for  the  single  actuator  case,  there  will  be  a  need  to  adjust 
controller  paraaeters  as  a  function  of  the  aean  operating  conditions  to  achieve 
high  operating  efficiency  and  good  dynaaic  response  of  the  velocity  servoaechan- 
isas. 


The  controller  analysis  and  design  problem  is  aore  coaplex  for  the  series- 
connected  actuators  because  of  the  wltivariabli;  nature  of  the  systea  dynaaics. 
Available  frequency  response  techniques  for  multivariable  control  system 
analysis  and  design  should  however  prove  to  be  applicable  here  [17]. 

CONCLUSIONS 

The  above  analysis  is  useful  in  evaluating  the  drive  circuit  concept 
described.  It  indicates  a  weakness  at  low  load  and  high  speed  -  an  important 
condition  for  this  application.  Moreover,  the  series  connection  of  the  actuators 
in  this  drive  circuit  concept  would  pose  problems  in  walking  gaits  other  than  the 
tripod  gait.  An  alternative  drive  circuit  concept  described  by  Haldron  et  al. 

[10]  uses  a  bleed  rate  control  configuration  and  should  be  superior  to  the  drive 
circuit  described  here  for  both  these  conditions  of  operation.  However,  its  dy¬ 
naaic  response  is  expected  to  be  inferior  to  that  of  the  drive  circuit  described 
here.  The  analytical  and  simulation  study  will  be  extended  to  the  alternative 
drive  circuit  concept  to  allow  a  proper  comparison  of  both  designs. 

The  analysis  and  simulation  results  presented  indicate  that  the  controller 
design  problem  for  the  velocity  servomechanisms  becomes  more  complex  as  a  result 
of  two  features  of  the  hydraulic  system  designed  to  improve  mechanical  efficiency- 
servovalve  operation  at  low  pressure  drops  and  the  series  connection  of  drive  ac¬ 
tuators  for  load  distribution.  However,  controller  adjustment  as  a  function  of 
the  mean  operating  conditions  does  enable  higher  operating  efficiencies  without 
degrading  system  dynamic  response.  The  study  also  suggests  appropriate  techniques 
for  dynamic  system  analysis  and  controller  design. 

Because  of  the  multiple  functions  required  of  each  hydraulic  circuit,  the 
design  of  these  circuits  is  exceedingly  coaplex  and  very  specific  to  the  appli¬ 
cation.  Although  not  applicable  to  other  types  of  robots  at  present,  these  ideas 
can  be  expected  to  become  iiaportant  as  new  and  laore  complex  robot  concepts  are 
developed.  Expected  developments  in  dynamic  systea  analysis  and  controller 
design  will  however  be  relevant  to  other  current  robotic  applications. 
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APPENDIX  A 

Wo  wish  to  derive  a  relationship  between  the  excess  force  AF(t)  at  the  drive 
actuator  and  the  velocity  and  acceleration  of  the  actuator  piston  and  the  vehicle 
center  of  mass. 


In  general,  the  relationship  between  the  vehicle  acceleration  A  and  the 
actuator  velocity  xp  and  acceleration  xp,  for  a  vehicle  using  a  linkage  leg  of 
the  type  shown  in  Figure  1,  is 


A 


Vo  pi 


CA-1) 
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Vg  and  Aq  are  coaputed  noainal  vehicle  velocity  and  acceleration  and  Vg  and  a^ 
are  the  corresponding  values  of  the  actuator  velocity  and  acceleration,  for  a 
specified  actuator  position. 

Assuming  conservation  of  nechanical  energy  and  neglecting  changes  in  kinetic 
and  potential  energy  due  to  changes  in  the  linkage  geoaetry  and  in  the  elevation 
of  the  vehicle  center  of  aass,  the  force  at  the  actuator  for  a  vehicle  aoving  on 
an  incline,  is 


MV 

F(t)  -  Fj^^(t)  ♦  AF  (t)  -  (A  ♦  g  sin  a)  (A-2) 

o 

a  is  the  angle  of  the  incline  and  M  is  the  share  of  vehicle  mass  seen  by  the 
actuator.  M  depends  therefore  on  the  positions  of  all  the  legs  in  contact  with 
the  ground. 


Coabining  equations  (A>1)  and  (A-2}  and  noting  that 


MV 

(A^  ♦  g  sin  a) 
o 


we  get 


MV  V  A 

^F(t}  (t)  ^(t) 

0  0*^  o  * 


V  a 

o  o  ;  2 


pi 


CO  -  AJ 


(A-3) 


(A-4} 


In  general,  the  determination  of  M  requires  that  the  equations  of  motion  of  the 
entire  vehicle  be  solved  simultaneously  to  determine  the  leg  positions  and  then 
the  leg  loads.  For  the  present  paper,  the  leg  load  distribution  corresponding 
to  tripod  gait  is  used  to  determine  M.  For  the  center  legs,  M  is  one-half  the 
vehicle  mass. 


The  simplest  linearized  form  of  equation  (A-4)  is  obtained  by  assuming  that 
the  vehicle  moves  in  tripod  gait  at  constant  speed  Vq  and  that  the  corresponding 
actuator  acceleration  a^  is  negligible.  Figure  3  indicates,  however,  that  this 
is  not  really  the  case. 

MV  ^ 

iFCt)  .  -rjYr  v(t)  (A-S) 

'  O  B  ^ 
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Appendix  3 

/««««»«  FILES  06LF34.PA8  *««»$«/ 


/* 

*/ 

/*  FUNCTION) 

THIS  FILE  CONTAINS  GLOBAL  DECLARATIONS  FOR 

*/ 

/* 

ROBOT  3.4.  THE 

EXECUTABLE  FILES 

WHICH  SHARE 

*/ 

/* 

THESE  QLOBALS 

ARE) 

*/ 

/» 

*/ 

/* 

R0BT34.PAS 

PLAN34.PAS 

F00T34.PAS 

*/ 

/* 

INIT34.PAS 

LINE34.PAS 

SERU34.PAS 

»/ 

/* 

0LNK34.PAS 

LIBR34.PAS 

%/ 

/* 

%/ 

CONST 


PI 

a 

3.14159) 

FSCALE 

a 

-200.0) 

/* 

FORCE  SCALE  FACTOR 

*/ 

HAXSTROKE 

«  12.0) 

/» 

HAXIHUN 

FOOT  STROKE 

*/ 

NOUE 

a 

TRUE) 

/» 

SWITCH 

TO  ENABLE  SERVOING 

*/ 

NOHOVE 

a 

FALSE) 

/* 

SWITCH 

TO  DISABLE  SERVOING 

*/ 

TYPE 

ARRAYS  -  ARRAYC1..6]  OF  REALS 
ARRAY18  -  ARRAYCO.a?]  OF  REAL) 

HODETYPE  -  <  RANDONt  NEUTRAL »  PREUALKt 

CRUISE>  SIDESTEP r  TURN  >1 


UAR 


HIDSTX 

t  NIDSTYf 

/t 

HIDSTANCE  COORDINATES 

*/ 

RPHASEt 

/* 

RELATIVE  LEG  PHASES 

*/ 

XFA> 

YFA. 

ZFAt 

/* 

ACTUAL  FOOT  FORCES 

*/ 

XFDf 

YFDf 

ZFD» 

/» 

DESIRED  FOOT  FORCES 

»/ 

XFF. 

YFFt 

ZFF, 

/* 

filtered  FOOT  FORCES 

*/ 

XPAf 

YPAi 

ZPA. 

/* 

ACTUAL  FOOT  POSITIONS 

*/ 

XPDi 

YPDi 

ZPOf 

/• 

DESIRED  FOOT  POSITIONS 

»/ 

XROf 

YRDf 

ZRDf 

/* 

DESIRED  FOOT  RATES 

»/ 

ZPTERH 

/» 

Z  POS.  TERH  FOR  ATT.  CONTROL 

*/ 

) 

ARRAY4) 

ZEROFORCE 

/t 

TRUE  FORCE  OFFSETS 

*/ 

9 

• 

ARRAY18) 

FZERO 

/* 

FLAGS  FOR  FORCE  ZEROING 

*/ 

:  ARRAYCt..«}  OF  BOOLEAN) 
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ATTITUDE. 

/» 

SWITCH  FOR  ATTITUDE  CONTROL 

*/ 

COMPLIANCE. 

/* 

INDIRECT  COMPLIANCE  SWITCH 

»/ 

OPTIMIZATION. 

/* 

SWITCH  FOR  OPTIMAL  FORCE 

$/ 

PASSli 

/* 

SWITCH  TO  INITIALIZE  FILTER 

SAVE. 

/* 

SWITCH  FOR  DATA  AQUISITION 

*/ 

SPRING 

/• 

SWITCH  FOR  ACTIVE  COMPLIANCE 

*/ 

t  BOOLEANf 

COMMAND 

/* 

OPERATOR  INPUT  COMMAND 

*/ 

{  CHARf 

LASTCLOCK. 

/* 

STORAGE  FOR  CLOCK  BUFFER 

*/ 

TOTALCLOCK 

/* 

CLOCK  TICK  ACCUMULATOR 

«/ 

:  INTEGER! 

MODE 

/* 

HEXAPOD  OPERATING  MODE 

*/ 

:  MQDETYPEI 

BETA. 

/* 

LEG  DUTY  FACTOR 

«/ 

DPS  I. 

/* 

FILTERED  TURN  RATE  COMMAND 

»/ 

DT. 

/t 

DELTA  TIME  (SEC) 

%/ 

FOOTtIFT. 

/* 

FOOT  LIFTING  HEIGHT 

%/ 

HIDSTZ. 

/* 

HIDSTANCE  Z  COORDINATE 

%/ 

NVELX.  NVELY. 

OPERATOR  VELOCITY  COMMANDS 

%/ 

NDPSI. 

/» 

OPERATOR  TURN  RATE  COMMAND 

%/ 

PERIOD. 

/* 

PERIOD  OF  KINEMATIC  CYCLE 

%/ 

PHASE. 

/* 

KINEMATIC  CYCLE  PHASE 

*/ 

RADIUS. 

/% 

RADIUS  FROM  CO.  TO  HIDSTANCE 

*/ 

SPERIOD* 

/% 

SUPPORT  PERIOD  (SEC) 

*/ 

VELX.  VLLY. 

/% 

FILTERED  VELOCITY  COMMANDS 

*/ 

VELMAX 

/% 

MAX  FOOT  VELOCITY  COMPONENT 

»/ 

:  REAL! 


SUPPORT 


/*  SET  OF  LEGS  IN  SUPPORT  PHASE  */ 
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/«  date:  30-HAR-S2  «/ 

/*  */ 
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/c  LINK  FILESi  LIBR34  «/ 

/*  PLAN34  '  */ 

/«  FGDT34  «/ 
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VAR  DPSIMAX  {READ  /*  MAXIMUM  TURN  RATE  */ 
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BETANODE  $-4* 

/«$«  ASSIGN  MIDSTANCE  POSITION  *«*/ 

MIDSTXCll  t-  22. 7S  4  1.434*  MIDSTXI23  22.75  4  1.436* 
NIDSTXC3]  *■  1.436*  MIDSTXC4I  :■  1.436* 

HIOSTXCSI  *-  -22. 7S  4  1.436*  HIDSTXC6I  :•>  -22.75  4  1.436* 

NID8TYC1I  )■  -24.0*  HIDSTYC2]  t-  24.0* 

HID8TYC3I  *■  -24.0*  NIDSTYC4]  *-  24.0* 

HID8TYCSI  t-  -24.0*  NID8TYC6]  t-  24.0* 

HIDSTZ  17.0* 

RADIUS  i-  SORT*  MIDSTXCll  «  MIDSTXCll  4  MIDSTYCll  «  MIDSTYC13  )* 
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/«»«  NEXTCONHAMO  DECLARATION  »»«/ 


NEXTCONC  RANDON  3 
NEXTCOHC  NEUTRAL  3 
NEXTCONC  PREUALK  3 
NEXTCONC  CRUISE  3 
NEXTCONC  TURN  3 
NEXTCONC  SIDESTEP  3 


C'I't'N'f'N'.'X'3» 
C'D'f'I'.'N'.'N'f 'U'»'X'3» 
C'C'.'M't'I't'N'»'N'i'T'»'X'.'Z'3» 
C'R'.'F'f 'H'f'L'.'P'»'R'»'S'»'X'3l 
C'H'r'P'f 'S'f 'X'3» 
C'H'»'L'»'R'.'X'3» 


HALTSET 


S-  C'N't'N'f 'I'*'M't'U'*'D'f 'C'.'T't'Z' 


'M'3I 


URtTE<  CHR<27>i  'C2J'>I  /*  ERASE  SCREEN  */ 

URITE(  CHR(27)t  'Cl»lH')t  /«  CURSOR  TO  HONE  «/ 

TE8TONI  /«  POWER  UP  THE  HEXAPOD  «/ 

TURNOFt  /«  SWITCH  OFF  NOTOR  POWER  UNTIL  NEEDED  «/ 


/«  DISPLAY  NENU  */ 

WRITE(  CHR<27)i  'C2J')I  /t  ERASE  SCREEN  «/ 

WRITE!  CHR(27)>  'CUIH')}  /«  CURSOR  TO  HONE  »/ 


WRITELN! 'ENTER 

N 

TO 

NORNALIZE  THE  LEO  POSITIONS 

BETANODE  • 

')! 

WRITELN!' 

U 

TO 

NOME  THE  lOOY  UP 

'  )» 

WRITELNi' 

D 

TO 

NOUE  THE  BODY  DOWN 

FOOTLIFT  ■ 

')» 

WRITELN! ' 

I 

TO 

INITIALIZE  THE  LEG  POSITIONS 

')» 

WRITELN! ' 

N 

TO 

NODIFY  THE  PARANETERS 

' ) » 

WRITELN! ' 
WRITELNi' 

X 

TO 

EXIT  THE  PROORAH  EXECUTION 

CONPLIANCE 

')» 
' )  t 

WRITELN!' 

C 

TO 

ENTER  CRUISE  NODE 

OPTINIZATION')) 

WRITELN! ' 

T 

TO 

ENTER  TURN- IN-PLACE  NODE 

'  > » 

WRITELN! ' 
WRITELN! ' 

Z 

TO 

ENTER  SIDESTEP  NODE 

ATTITUDE 

'  )  1 
'  >» 

WRITELN! ' 

F 

TO 

INCREASE  FORWARD  VELOCITY  CONPONENT 

')! 

WRITELN! ' 

8 

TO 

INCREASE  REARWARD  VELOCITY  CONPONENT 

AQUISITION 

'  )l 

WRITELN! ' 

S 

TO 

INCREASE  CLOCKWISE  TURN  RATE 

TINE 

'  )  1 

WRITELNI ' 

P 

TO 

INCREASE  COUNTERCLOCKWISE  TURN  RATE 

' ) » 

WRITELN!' 

R 

TO 

INCREASE  RIOHTWARD  VELOCITY  CONPONENT 

' )  1 

WRITELN! ' 

L 

TO 

INCREASE  leftward  VELOCITY  CONPONENT 

' )  1 

WRITELN!' 

WRITELNI 

H 

TO 

HALT  NOTION 

'  )l 
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/«»«  SET  TERHINAL  CHARACTERISTICS  «««/ 

URITE(  CHR(27>t  'CT41'  )l  /*  SET  TO  NOSCROLL  «/ 

URITE<  CHR(27)f  'C20l22r'  )t  /«  SET  SCROLLING  REGION  «/ 

HRITE<  CHR(27't  'CT61'  >>  /«  SET  ABSOLUTE  ORIGIN  NODE  »/ 

HR1TE<  CHR(27>f  'C20tlf'  >*  /»  CURSOR  TO  SCROLLING  REGION  «/ 

KEYINTI  <  PLACE  KEYBOARD  INPUT  REQUEST  ON  QUEUE  > 

CLOCKINITI  <  START  PROQRANHABLE  CLOCK  > 


/«  REAL  -  TINE  OPERATION  •/ 

COHNANO  M't  <  ENTER  HODIFY  SEQUENCE  INITIALLY  > 

NODE  :■  RANDOM I 

REPEAT  {  UNTIL  COMMAND  -  'X'  > 

URITE(  CHR(27)t  'C22I1H'  )l  <  CURSOR  TO  SCREEN  BOTTOM  > 

URITE<  CHR<27)«  'COa'  >»  <  NORMAL  CHARACTER  ATTRIBUTES  > 

URITELNI 

URITELN<  COMMAND  )t 

IF  NOT  <  COMMAND  IN  NEXTCONC  NODE  I  > 

THEN  <  COMMAND  IS  INVALID  > 

BEGIN 

URITELM<  CHR<I3>>  CHR(27 ) • ' »««»«  INVALID  COMMAND  **»'>! 
URITE<  'VALID  COMMANDS  ARES  '  >» 

FOR  LETTER  'A'  TO  'Z'  DO 

IF  LETTER  IN  NEXTCONC  MODE  3  THEN  WRITE('  ' (LETTER) > 
MRITELN) 

IF  NODE  IN  C  RANDOM*  NEUTRAL*  PREHALK  3 
THEN  COMMAND  t-  'H'  <  WAIT  > 

ELSE  COMMAND  t-  'G'l  <  GO  > 

END) 

<  END  IF  > 
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IF  COHNAND  IN  HALT8ET 
THEN 

BEGIN 

CASE  COHHAND  OF 

'N't  BEGIN 

HORHALIZEI 
NODE  t>  NEUTRAL! 

END! 

'IM  BEGIN 

NGRHALIZEI 

INITIALIZE! 

NODE  t-  PREMALK! 

END! 

'U't  UPOOWNt  24.3  )! 

'DM  UPOOHN<  HI03TZ  >! 

'H'i  BEGIN 

HALT! 

NODE  t<*  PREUALKI 

SAUE  !«  FALSE!  <  SUITCH  OFF  DATA  AOUISITION  > 

END! 

'C':  BEGIN 

TURNON! 

URITELN! 

URITELN<  CHR(27>r  'BABB*  CRUISE  NODE  BBB')! 

NODE  :•  CRUISE! 

SPRING  :>  CONPLIANCEI  <  ENABLE  ACTIVE  COMPLIANCE  > 
PASSI  t-  TRUE!  <  FLAG  FOR  FILTER  INITIALIZATION  > 
END! 

'T't  BEGIN 

TURNON! 

URITELN! 

URITELN!  CHR<27)»  'tABBB  TURN-IN-PLACE  NODE  BBB')! 
NODE  t-  TURN! 

SPRING  !-  COMPLIANCE!  <  ENABLE  ACTIVE  COMPLIANCE  > 
PASSI  !■  TRUE!  <  FLAG  FOR  FILTER  INITIALIZATION  > 
END! 

'Z'S  BEGIN 

TURNON! 

URITELN! 

URITELN!  CHR!27>>  'BABBB  SIDESTEP  NODE  BBB')! 

NODE  :«  SIDESTEP! 

SPRING  t-  COMPLIANCE!  <  ENABLE  ACTIVE  COMPLIANCE  > 
PASSI  t-  TRUE!  <  FLAB  FOR  FILTER  INITIALIZATION  > 
END! 
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'H' t 


<«««> 


<«««> 


1 


BEGIN 

lOKILLt  <  CANCEL  INPUT  REQUEST  > 

URITELNi 

URITELN<  CHR<27)t  'BAPARAHETER  MODIFICATION' ) » 

URITELNI 

URITELNI  'CHANGE  BETA?' > I 
READLNI  LETTER  >1 
IF  LETTER  •  'T'  THEN 
REPEAT 

URITELNI 'PLEASE  ENTER  DESIRED  BETANODEt')! 

URITELNI'  1!  BETA  -  S/A  21  BETA  •  3/4') I 

URITELNI'  3t  BETA  >  2/3  41  BETA  -  1/2') I 

REABLNIBETAHODE)! 

UNTIL  BETAHODE  IN  Clt2i3i4]t 
URITEI  CHRI27)>'7'f  CHRI 27 > > ' C 1 1 A7H' >  CHRI27) f ' Cl«'  )l 
URITEI  BETAHODEtlt  CHRI27)f  '8'  )l 
URITELNI 
URITELNI 

CASE  BETAHODE  OF 

i:  BETA  0.83331  <  S/A  > 

21  BETA  t-  0.7SI  <  3/4  > 

3:  BETA  :>  0.AAA7I  <  2/3  > 

41  BETA  :■  0.5  <  1/2  > 

ENDI/«  CASE  «/ 

/««•  ASSIGN  RELATIVE  LEO  PHASES  t«»/ 

RPHA8EC13  t-  0.01 

RPHASEC23  O.SI 

RPHASEC33  l>  BETAI 

RPHA8EC43  I-  BETA  -  0.51 

RPHASECS3  l>  2.0  «  BETA  -  1.01 

RPHASECA3  t-  RPHASECS3  4  0.51 

RPHA8ECA3'i-  RPHASECA3  -  TRUNCIRPHA8ECA3 ) I 

URITELNI  'CHANGE  FOOTLIFTT'  )l 
READLNI  LETTER  >1 
IF  LETTER  •  'Y'  THEN 
REPEAT 

URITELNI  'ENTER  NEU  VALUE  OF  FOOTLIFT'  )l 
READLNI  FOOTLIFT  >1 

UNTIL  I  FOOTLIFT  >-  1.0  )  AND  I  FOOTLIFT  <-  12.0  )l 
URITEI  CHRI27).'7'»  CHR ( 27 )  . ' t3l A7H' .  CHRI 27 ) » ' C !■'  )l 
URITEI  F00TLIFTS4ilf  CNRI27)>  '8'  >1 
URITELNI 
URITELNI 

VTHAX  ZRHAX  «  HAXSTROKE  /  I  FOOTLIFT  «  PI  )l 
IF  VTNAX  >  4.0  THEN  VTHAX  !■  4.01  <  IN.  PER  SEC  > 
VELHAX  S-  VTHAX  •  I  l.O  -  BETA  )  /  BETAI 
DPSIHAX  VELHAX  /  RADIUS! 
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<t$«>  yRITELN<  'TURN  ACTIVE  COMPLIANCE  ONT')» 
READLN<  LETTER  >i 

URITE(  CHR(27)t  '7'>  CHR(27)>  'C6tSAH'  )( 
IF  LETTER  »  'Y' 

THEN 

BEGIN 

COMPLIANCE  TRUE> 

URITE(  CHR(27>>  'C7ISila'  >i 
END 

ELSE 

BEGIN 

COMPLIANCE  FALSER 

URITE<  CHR(27>r  'COa'  >1 
END  I 

URITE(  'COMPLIANCE'*  CHR(27>»  '8'  >1 

URITELNt 

URITELNI 

<»««>  URITELNI  'TURN  FORCE  OPTIMIZATION  ON?')i 
READLNI  LETTER  >) 

URITE<  CHR(27)f  '7't  CHR(27>.  't8IS6H'  >; 
IF  LETTER  ■  'Y' 

THEN 

BEGIN 

OPTIMIZATION  TRUE> 

URITEt  CHR(27)>  'C7(S;ia'  )» 

END 

ELSE 

BEGIN 

OPTIMIZATION  >>  FALSE) 

URITEi  CHR(27)>  'COa'  >) 

END  I 

URITEI  'OPTIMIZATION'*  CHR(27>*  '8'  )> 

URITELNI 

URITELNI 

{$««>  URITELNI  'TURN  ATTITUDE  CONTROL  ONT'li 
READLNI  LETTER  >1 

URITEI  CHRI27)*  '7'*  CHRt27>*  'CIOISAH'  ) 
IF  LETTER  -  'Y' 

THEN 

BEGIN 

ATTITUDE  I-  TRUE  I 

URITEI  CHRI27>>  'C7l5lla'  )l 

END 

ELSE 

BEGIN 

ATTITUDE  t-  FALSEI 
URITEI  CHR(27>*  'COa'  >1 
END  I 

URITEI  'ATTITUDE'*  CHR(27>*  'B'  >1 
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IHQUEI  <  REASSERT  INPUT  REQUEST  > 

NODE  t-  RANDOHI 

WRITELNI 

URITELNI 

URITELNI  CHR<27>>  'RAINITIALIZE  HEXAPOD  ')* 
END  I 


'H*  J 

END I  <  CASE  CONNAND  > 

KYUAITt 

KEYCK(CQHHAND>t 

DT  t>  DEL.TAT1HEI  <  UPDATE  LASTCLOCK  AFTER  WAIT  > 
END  <  THEN  > 


ELSE 

BEGIN 

CASE  COMMAND  OF 


'F' : 

IF 

NVELX 

<  (  0.9  « 

:  VELNAX  > 

THEN 

NVELX 

NVELX  *  ( 

0.1 

« 

VELNAX  ) 

ELSE 

NVELX  i- 

VELNAX  t 

'B'l 

IF 

NUELX 

>  -(  0.9 

t  VELNAX 

> 

THEN 

NVELX  t- 

NVELX  -  ( 

0.1 

« 

VELNAX  > 

ELSE 

NVELX  t- 

-VELNAX > 

'S'l 

IF 

NDPSI 

<  <  0.9  * 

DPS I MAX 

) 

THEN 

NDPSI  i> 

NDPSI  f  < 

0.1 

t 

DPSINAX  > 

ELSE 

NDPSI  t- 

DPS I MAX i 

'p' : 

IF 

NDPSI 

>  -(  0.9 

«  DPSINAX 

> 

THEN 

NDPSI  t- 

NDPSI  -  ( 

0.1 

« 

DPSINAX  ) 

ELSE 

NDPSI  t> 

-DPSINAX > 

'R' J 

IF 

NVELY 

<  (  0.9  1 

:  VELNAX  > 

THEN 

NVELY  1- 

NVELY  *  ( 

0.1 

VELNAX  > 

ELSE 

NVELY  !• 

VELNAX 1 

'L' J 

IF 

NVELY 

>  -(  0.9 

B  VELNAX 

> 

THEN 

NVELY  «■ 

NVELY  -  t 

0.1 

« 

VELNAX  > 

ELSE 

NVELY  :■ 

-VELNAX 1 

'O' : 


END I  <  CASE  CONNAND  > 

PLANMOTIONI  <  NOTION  PLANNINO  t  CXECUTION  > 
<  UNTIL  NEXT  CONNAND  INPUT  > 


END I  <  ELSE  > 

<  END  IF  CONNAND  IN  HALTSET  > 
UNTIL  COMMAND  -  'X'i 
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HALT  I 


lOKlLLI  <  CANCEL  KEYBOARD  INPUT  REQUEST  > 

URITE(  CHR(27)f  'C?4h')>  .  /«  SET  TO  SHOOTH  SCROLL  «/ 

URITEC  CHR(27)i  'Cll22r'>»  /«  RESTORE  SCROLLING  REGION  «/ 

WRITE!  CHR(27)t  'C2J'>I  /»  ERASE  SCREEN  »/ 

WRITE!  CHR!27)f  'ClIIH')!  /*  CURSOR  TO  HONE  */ 

END. 
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(ttEiC) 


/««««•««  file:  PLAN3S.PAS 


/««««««««««««»*««««««««*««»«»«/ 

/$  PROCCOUREt  PLANHOTION  */ 


/«« 

!««««•«»««$/ 

/* 

*/ 

/* 

PROORANNERS:  TED 

CHANG t  DENNIS  PUOH 

*/ 

/« 

DATE!  30-NAR-82 

*/ 

/* 

FUNCTION!  PLAN 

BODY  NOTION  TO  INPLENENT  OPERATOR 

CONNANDS  «/ 

/« 

USER  OUIDE!  THE 

CALLING  FORNAT  IS! 

*/ 

/* 

PLANNOTION! 

*/ 

/* 

%/ 

/* 

PROCEDURES  CALLED!  FOOTLINEt  KEYCK 

*/ 

/« 

*/ 

/* 

GLOBAL  VARIABLES! 

*/ 

/« 

REFERENCED! 

NVELXt  NVELY.  NDPSI 

*/ 

/« 

BETA.  VELNAX 

t/ 

/* 

NODE 

*/ 

/t 

•/ 

/• 

NODIFIED! 

VELXf  VELYr  DPSI 

«/ 

/* 

PERIOOt  SPERIODt  PHASE 

*/ 

/* 

DT 

*/ 

/« 

*/ 

PROCEDURE  PLANHOTIONt 


CONST  riNECONST  >  O.S< 
NINSTROKE  -  S.OI 
RNIN  >  60. Ot 


VAR  DVELXf  DVELYf  ODPSI> 
NVEL> 

LTIHEf 

FOOTRATE> 

STROKE 


PROCEDURE  FOOTPATH I  EXTERNAL! 

PROCEDURE  KEYCK(  VAR  COHNAND!  CHAR  >1  FORTRAN! 
FUNCTION  SION<  Xi  REAL  >3  REAL!  EXTERNAL! 
FUNCTION  DELTATIHE*.  REAL!  EXTERNAL! 


/$  TINE  CONST.  FOR  INPUT  FILTER  »/ 
/*  HINIHUH  FOOT  STROKE  »/ 
/$  HIN.  TURN  RADIUS  <  CRUISE  >  */ 


/«  VELOCITY  DERIVATIVE  TERNS  */ 
/*  VELOCITY  CONHAND  NA6NITUDE  t/ 
/«  TENPORARY  STORAGE  FOR  TINE  «/ 
/*  APPROX.  FOOT  RATE  URT  BODY  */ 
/%  FOOT  TRAVEL  IN  SUPPORT  PHASE  «/ 

(REAL! 
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BEGIN  <  PLANHOTION  > 

IF  NODE  -  CRUISE  <  SET  LIMITS  ON  VELOCITY  COMPONENTS  > 

THEN 

BEGIN 

HRITELN<  CHR(27>f  'COM'  )l  <  NORMAL  CHARACTER  ATTRIBUTES  > 

/«  SET  LIMIT  FOR  CRAB  ANGLE  IN  CRUISE  MODE  »/ 

IF  ABS(  NVELY  )  >  ABS(  NVELX  > 

THEN  NVELY  1-  SIGN<  NVELY  )  »  ABS(  NVELX  )t 

/«  DETERMINE  MAGNITUDE  OF  COMMANDED  VELOCITY  »/ 

NVEL  t-  NVELXBNVELX  *■  NVELY«NVELY» 

IF  NVEL  <>  0.0  THEN  NVEL  i-  SQRT(  NVEL  )t 

/«  LIMIT  VELOCITY  MAGNITUDE  TO  VELMAX  «/ 

IF  NVEL  >  VELMAX  THEN 
BEGIN 

NVELX  t>  NVELX  /  NVEL  «  VELMAX < 

NVELY  i-  NVELY  /  NVEL  «  VELMAX! 

END! 

/«  SET  LIMIT  FOR  DPSI  IN  CRUISE  MODE  */ 

IF  ABS(  NDPSI  )  >  ABS(  NVEL  /  RMIN  > 

THEN  NDPSI  SIGN!  NDPSI  >  «  ADS(  NVEL  /  RMIN  )! 

URITE(  NVELX:eS3>  NVELY!8t3>  NDPSIt8t3  >i 
IF  ABStNDPSI)  >  0.00001 

THEN  URITE<  NVEL  /  ABS(  NDPSI  >!8:2>! 

END  <  THEN  > 


ELSE 

WRITE!  NVELX:8!3t  NVELY;8>3>  NDPSIi8t3  )l 
<  END  IF  MODE  > 

WRITE!  CHR!27)f  'CIA'  >>  <  ENABLE  BOLD  CHARACTERS  > 


REPEAT  <  UNTIL  A  COMMAND  IS  INPUT  > 


/9**  GENERATE  DT  «««/ 
DT  DELTATIMEI 


/*««  FILTER  THE  RATE  COMMAND  INPUTS  ««*/ 


DVELX  t-  !NVELX  -  VELX)  /  TIMEC0N3T! 
DVELY  !■  !NVELY  -  VELY!  /  TIMECONSTI 
DDPSI  !NDPSI  -  DPSI)  /  TIMECONSTI 

VELX  i-  DVELXtDT  F  VELX! 

VELY  !■  OVELYBOT  F  VELY! 

DPSI  !-  DDPSIBDT  F  DPSI! 


/* 

LONGITUDINAL  ACCEL. 

*/ 

/* 

LATERAL 

ACCELERATION 

*/ 

/* 

TURNING 

ACCELATION 

»/ 

/* 

LONGITUDINAL  VEL. 

*/ 

/« 

LATERAL 

VELOCITY 

*/ 

/* 

TURNING 

RATE 

*/ 
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/t««  CALCULATE  STROKE  t  SUPPORT  PERIOD  «««/ 


CASE  HODE  OF 

CRUISE t  FOOTRATE  i-  VELXI 

SIDESTEP!  FOOTRATE  !-  VELYt 

TURN!  FOOTRATE  t-  RADIUS  «  DPSIi 

END  I  <  CASE  > 

IF  ABS(  FOOTRATE  >  >  0.00001 
THEN 

BEGIN 

STROKE  HINSTROKE  *  (  HAXSTROKE  -  HINSTROKE  ) 

«  ABS(  FOOTRATE  >  /  VELHAXi 

SPERIOD  STROKE  /  FOOTRATE!  <  SUPPORT  PERIOD  > 

END 

ELSE 

SPERIOD  :>  10000.0! 

PERIOD  t-  SPERIOD  /  BETA!  <  TOTAL  CYCLE  PERIOD  > 


/*««  UPDATE  PHASE  VARIABLE  *««/ 

.  PHASE  !-  PHASE  DT  /  PERIOD  *  1! 

PHASE  !>  PHASE  -  TRUNC ( PHASE ) ! 

/*»•  CALL  FOOT  TRAJECTORY  GENERATION  ROUTINE  ***/ 
FOOTPATH! 

/«»«  CHECK  FOR  OPERATOR  INPUT  «*»/ 

KEYCK(C0NHAN0>> 

UNTIL  ORD!  CONHAND  >  <>  0!  <  UNTIL  A  COHHAND  IS  INPUT  > 

END! 
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(«»E4«) 


file:  F00T34.PAS  ««««$»«/ 


/«  procedure:  footpath  «/ 


/* 

PROGRANNERS:  DENNIS 

PUGHt  TED  CHANG 

*/ 

/* 

DATE)  30-HAR- 

-82 

*/ 

/« 

*/ 

;* 

function:  CALCULATES  FOOT  TRAJECTORIES  TO 

IMPLEMENT 

*/ 

/* 

THE  BODY  RATES  COHHANDED  PY  BODY 

MOTION 

$/ 

/* 

PLANNll^G. 

%/ 

/* 

»/ 

/* 

USER  guide:  THE  CALLING  FORMAT  IS:  FOOTPATH) 

%/ 

/* 

«/ 

/t 

*/ 

/* 

PROCEDURES  CALLED: 

JSERVOf  RBADC 

»/ 

/« 

%/ 

/« 

GLOBAL  VARIABLES 

%/ 

/t 

referenced: 

VELXf  VELY.  DPSI 

*/ 

/* 

MIDSTXt  MIOSTY,  MIDSTZ 

»/ 

/* 

DT»  PERIOD.  SPERIOD 

*/ 

/* 

PHASE.  RPHASE.  BETA 

»/ 

/« 

FOOTLIFT 

*/ 

/* 

*/ 

/* 

modified: 

XPD.  YPD.  ZPD 

*/ 

/« 

XRD»  YRD.  2R0 

*/ 

/* 

XF0.  YFD.  ZFD 

*/ 

/* 

SUPPORT.  ZPTERM 

»/ 

/• 

%/ 

PROCEDURE  FOOTPATHI 

CONST  ATTSCALE  ■  -0.7B54I  /*  ATTITUDE  SCALE  FACTOR  «/ 

ATTPOLE  -  8.0»  /*  POLE  FOR  ATTITUDE  CONTROL  */ 

FTOTAL  «  285. 0»  /t  TOTAL  VEHICLE  HEIGHT  (  LBS.)  */ 

PWRSCALE  ■  -33333.3)  /*  POWER  SCALE  FACTOR  «/ 
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VAR 

ALPHAi  /«  CRAB  ANGLE  URT  LONGITUDINAL  AXIS  */ 

CGXf  CQY»  /t  COORDINATES  OF  CG.  PROJECTION  «/ 

DTI  I  /«  BACKED  UP  TINE  FRON  HID8TANCE  */ 

DXBt  DYB>  /«  BODY  DISPLACEMENTS  IN  BODY  COORD.  (/ 

DXBlt  DYBli  /«  TOUCHDOWN  DISPL.  FROM  MIDSTANCE  «/ 

DXEt  DYE>  /«  BODY  DISPLACEMENT  IN  EARTH  COORD.  */ 

LPHASE.  /«  LEG  PHASE  VARIABLE  «/ 

PITCH.  ROLL.  /«  ACTUAL  ANGLES  FRON  GYRO  «/ 

PPITCH.  PROLL.  /t  ACTUAL  ANGLES  FRON  PENDULUM  */ 

POWER.  /«  HEXAPOD  INSTANTANEOUS  POWER  CONSUME.  «/ 

PSIC.  PSICI.  f%  BODY  ANGULAR  DISPLACEMENT  OVER  DT  »/ 

Q.  R.  S.  /«  INTERNED.  FORCE  OPTINIZATION  TERNS  »/ 

SUMX.  SUMY.  /«  SUN  OF  FOOT  DISPLACEMENTS  «/ 

SUHX2>  SUMY2.  /«  SUN  OF  SQUARES  OF  FOOT  DISPLACEMENTS  %! 

SUMXY.  /*  SUM  OF  FOOT  DISPL.  CROSS-PRODUCTS  •/ 

TIMEFP.  /*  REMAINING  TIME  IN  TRANSFER  PHASE  »/ 

TPHASE.  /«'  TRANSFER  PHASE  VARIABLE  «/ 

TRTIME.  /«  TOTAL  TIME  IN  TRANSFER  PHASE  «/ 

VEL.  /«  MAGNITUDE  OF  VELOCITY  VECTOR  «/ 

ZOFF.  /•  Z  POSITION  ERROR  FOR  ATT.  CONTROL  */ 

ZRTERM  /«  Z  RATE  OFFSET  FOR  ATTITUDE  CONTROL  */ 

:  REAL) 

FRSTCH.  /«  FIRST  A/D  CHANNEL  FOR  DATA  FETCH  «/ 

I.  /«  LOOP  COUNTER  «/ 

N  /«  SUPPORT  PHASE  LEG  SET  COUNTER  */ 

:  INTEGER! 

XFTHLD.  YFTHLO  /«  DESIRED  FOOT  TOUCHDOWN  POINT  */ 

:  ARRAYS! 

rOPBLOCK  /*  TOP  A/D  CHANNELS  */ 


:  ARRAYI8! 

PROCEDURE  JSERVOI  MOVE!  BOOLEAN  >!  EXTERNAL! 

PROCEDURE  RBAOC(  NOCHAN.  FRSTCH!  INTEGER!  SCALE!  REAL!  VAR  tHDATA!  ARRAYIG  )! 
EXTERNAL! 


FUNCTION  ATAN2(Y.  X  !  READ!  REAL!  EXTERNAL! 


BEGIN  /«  F00T34  */ 

ft  EXPRESS  INCREMENTAL  BODV  DISPLACEMENT  IN  BODY  COORDINATES  «/ 

/««««««««««««««««««««»»«»«$*«»«««»««*««**«»«»*<«««*«*«$$«*«»««»«/ 

fttt  CALCULATE  INCREMENTAL  BODY  DISPLACEMENT  URT  GROUND  «««/ 

PSIC  !-  DPSI  «  DTt 

/«««  CALCULATE  MAGNITUDE  OF  VELOCITY  URT  GROUND  ««*/ 

VEL  S-  VELX  «  VELX  t  VELV  «  VELYI 

IF  VEL  <>  0.0  THEN  VEL  1-  SORT (VEL) I 

IF  ABS(OPSI)  >  0.00001 
THEN 

BEGIN 

DXE  t-  VEL/DPSI  t  SIN(PSIC>: 

DYE  VEL/DPSI  «  (1.0  -  C0S(PS1C>>) 

END 

ELSE 

BEGIN 

DXE  VEL  «  DT) 

DYE  O.Ot 

END! 

/«««  ROTATE  DISPLACEMENT  VECTORS  TO  BODY  COORDINATES  «««/ 

ALPHA  :>  ATAN2(  VELY>  VELX  >) 

OXB  :>  DXE  «  COS(ALPHA> 

-  DYE  «  SIN(ALPHA)I 

DYB  DXE  «  SIN(ALPHA) 

*  DYE  «  COS(ALPHA>t 


/*««««««««««»«««*««««««««*«$«»««*«*««/ 

ft  COMPUTE  FOOT  TOUCHDOWN  OFFSETS  «/ 

/«*««*««««*«««««««««««««««»««««««*«««/ 

/«$*  CALCULATE  BODY  DISPLACEMENT  FROM  MIOSTANCE  TO  TOUCHDOWN  tttf 
DTI  -O.S  «  ABS(SPERIOD) I  <  BACKED  UP  TIME  FROM  MIDSTANCE  > 

PSICl  :>  DPSI  «  DTII  <  BACKED  UP  ANGLE  > 

IF  ABS(OPSI)  >  0.00001 
THEN 

BEGIN 

DXE  t-  VEL  /  DPSI  «  SIN(PSIC1)I 

DYE  S>  VEL  /  DPSI  *  (  1.0  -  C0S(PS1C1>  )l 

END 

ELSE 

BEGIN 

DXE  VEL  «  DTlI 

DYE  :■  0.01 

ENDi 
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/*$«  ROTATE  TOUCHDOUN  OFFSET  TO  BODY  COORDINATES  *«»/ 
0X81  OXE  «  COS(ALPHA» 

-  DYE  «  SIN(ALPHA>I 

DYBl  OXE  t  SIN<ALPHA> 

*  DYE  «  COS< ALPHA)) 


RBA0C(StS8ilfT0PBL0CK) )  <  FETCH  PITCHi  ROLLi  1  POWER  FROM  VEHICLE  > 

PPITCH  :>  T0PBL0CKC4I  «  ATTSCALEI 
PROLL  t-  TOPBLOCKCSI  «  ATTSCALE) 

POWER  :>  T0PBL0CKC6]  «  PWRSCALE) 

PITCH  T0PBL0CKC7]  «  ATTSCALE) 

ROLL  :>  TOPBLOCKCai  «  ATTSCALE) 


/«  GENERATE  FOOT  TRAJECTORIES  «/ 

FOR  I  1  TO  6  DO  <  GENERATE  FOOT  COORDINATE  FOR  LEG  I  > 

BEGIN 

/«««  COMPUTE  LEG  PHASE  VARIABLE  ««*/ 

LPHASE  PHASE  *  RPHASECIJ  *  1.0) 

LPHASE  LPHASE  -  TRUNC<  LPHASE  )) 

IF  LPHASE  >  BETA 

THEN  <  LEO  IN  TRANSFER  PHASE  > 

BEGIN 

TPHASE  t-  (LPHASE  -  BETA)  /  (1.0  -  BETA)) 

SUPPORT  :«  SUPPORT  -  ClI)  <  REMOVE  LEG  I  FROM  SUPPORT  SET  > 

IF  <  LEO  AT  TOP  OF  TRANSFER  PHASE  AND  FORCE  NOT  YET  ZEROED  > 

(  (  (PERIOD  >  0.0)  AND  (LPHASE  >  ( BETA41 ) /2 . 0 )  ) 

OR  (  (PERIOD  <  0.0)  AND  (LPHASE  <  ( BETA41 ) /2 . 0 >  )  ) 

AND  (FZEROCI]  >  FALSE) 

THEN  <  UPDATE  OFFSET  FORCES  FOR  LEO  I  > 

BEGIN 

FRSTCH  :*  (  I  -  1  )  «  3) 

RBADC(  3r  FRSTCH.  FSCALEf  ZEROFORCE)) 

FZEROCI]  TRUE)  <  FLAG  THAT  LEG  I  IS  UPDATED  > 

ZPTERHCI]  t-  0.0)  <  INITIALIZE  ATTITUDE  CORRECTION  TERM  > 

END) 

<  END  IF  > 
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/••t  CALCULATE  TIHE  LEFT  TILL  TOUCHDOHN  OF  THE  LEO  *»«/ 
TRTIHE  (  1  -  BETA  >  •  ABS(PERIOD>) 

IF  PERIOD  >  0.0 

THEN  TIHEFP  :«  TRTIME  •  (  1.0  -  TPHA8E) 

ELSE  TIHEFP  i-  TRTIHE  »  TPHASEI 

/«««  COHPUTE  THE  BEST  TOUCHOOWH  POINT  ««*/ 

XFTHLOCIl  :>  (  HIDSTXCII  -  DXBl  >  «  COSCPSICl) 

i  (  HIOSTYCIl  -  DYBl  >  «  SIN(PSIC1>« 

YFTHLOCII  t-  -  <  HID8TXCI3  -  DXBl  )  *  SIN(P8IC1> 

4  (  HlDSTYCll  -  DYBl  >  «  C0S(PS1C1>< 

/«««  COHPUTE  DESIRED  FOOT  POSITION  «««/ 

IF  DT  <  TIHEFP 
THEN 


BEGIN 

XP0CI3 

!- 

XPDCI3  4  <XFTHLDCI3 

-  XP0CI3> 

« 

DT/TIHEFPI 

YP0CI3 

t- 

YPDCI3  4  <YFTHLDCI3 

-  YPDLID) 

« 

DT/TIHEFP* 

ZPDCI3 

!■ 

HID8T2  -  FOOTLIFT  * 

S1N(  TPHASE 

*  PI  >  * 

END 

BEGIN 

XPDCI3 

S- 

XFTHLDCIIt 

YPDCI3 

:■ 

YFTHLDCI3I 

ZP0CI3 

END! 

•  ■ 

HIDSTZI 

<  END  IF  DT  > 

/«««  COHPUTE  DESIRED  FOOT  RATE  ««*/ 

XRDCn  (  XFTHLDCn  -  XPDCI3  >  /  TIHEFPI 
YRDCn  J-  <  YFTHLDCI3  -  YPDII3  )  /  TIHEFP* 

ZRDtI3  :■  -  FOOTLIFT  •  PI  •  COS(  PI  «  TPHASE  > 

/  (  PERIOD*  (  1  -  BETA  )  )( 

END  <  TRANSFER  PHASE  > 

ELSE  <  FOOT  IN  SUPPORT  PHASE  > 

BEGIN 

SUPPORT  SUPPORT  4  CI3»  <  INCLUDE  LED  I  IN  SUPPORT  SET  > 

FZER0CI3  FALSE*  <  FLAG  THAT  FORCE  NOT  ZEROED  THIS  CYCLE  > 

/««*  CONFUTE  ATTITUDE  CONTROL  VARIABLES  «**/ 

ZOFF  -PITCH  *  XPACI3  4  ROLL  «  YPACI3I 
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IF  ATTITUDE  -  TRUE 
THEN 

BEGIN 

ZRTERN  ATTPOLE  «  ZOFFI 

ZPTERHCII  t-  ZPTERNCIl  F  ZRTERH  «  DT» 

END 

ELSE 

BEGIN 

ZPTERHCI3  0.01 
ZRTERN  t-  O.Ot 
ENDI 

<  END  IF  ATTITUDE  > 

/««t  CONFUTE  DESIRED  FOOT  POSITION  ««t/ 

XPDCI3  (  XPDCIl  -  DXB  >  «  COSCPSIC) 

F  <  YPDCn  -  DYB  >  «  SIN(PSIC>t 

YPDCI]  t-  -  (  XPDCI3  -  DXB  )  *  SINiPSIC) 

F  (  YPDCI3  -  DYB  )  «  COStPSIOl 

ZPDCI3  HIOSTZ  F  ZPTERMtI3t 

/«««  CONFUTE  DESIRED  FOOT  RATE  «««/ 

XRDCI3  t-  -VELX  F  DPSI  «  YPDCI3> 

YR0CI3  {•  -MELY  -  DPSI  «  XPDCI3> 

ZR0CX3  t-  ZRTERNI 

ENOt  /«  SUPPORT  PHASE  t/ 

/«  END  IP  LPHASE  «/ 

ENDI  /«  FOR  1 


/«  CONFUTE  OPTINAL  FORCE  SETPOINTS  */■ 


N  01 
SUNX  t-  0.01 
8UNY  S-  0.01 
SUNX2  <-  0.0* 
SUNY2  O.OI 

8UNXY  0.0> 


I 
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FOR  I  1  TO  6  DO 

IF  I  IN  SUPPORT  THEN 
BEGIN 

SUHX  i-  SUHX  *  XPACIlt 
SUHY  SUNT  t  YPACI3I 
SUNX2  SUHX2  4  XPACI3  *  XPACI3I 

SUNY2  t-  SUNY2  4  YPACt3  «  YPACI3> 

SUHXY  !■  SUHXY  4  XPACI3  «  YPACI3I 

N  N  4  II 

END!  <  IF  1  IN  SUPPORT  > 

<  END  FOR  I  > 

R  (SUHX  «  SUNY2  -  SUHXY  «  SUHY)  /  (SUHX2  »  SUHY2  -  SUHXY 

0  I-  (SUHY  -  SUHXY  «  R)  /  SUHY2I 

S  N  -  ((SUHX  «  SUHY2  -  SUHXY  *  SUHY)  «  R  4  SUHY  «  SUHY)  / 

COX  S-  -SIN(  PITCH  )  t  HIDSTZ) 

COY  S-  SIN(  ROLL  )  «  NID8TZI 

/*««  COHPUTE  FOOT  FORCE  SETPOINTS  «*«/ 

FOR  I  1  TO  6  DO 
BEGIN 

XFOCI3  t-  0.01 
YF0CI3  :>  0.01 

IF  I  IN  SUPPORT 
THEN 

IF  OPTINIZATION  •  TRUE 

THEN  2FDCI3  :•  (  1  -  0  *  (  YPAI13  -  CGY  ) 

-  R  «  (  XPACI3  -  COX  )  ) 
ELSE  ZFDCI3  I*  FTOTAL  /  N 

ELSE 

ZFDCI3  I-  0.01 
ENDI  <  FOR  I  > 

J8ERV0(  HOVE  >1  <  CALL  SERVO  ROUTINE  > 


ENDI  /«  FOOTPATH  */ 


«  SUHXY) I 
SUHY2I 


«  FTOTAL  /  S 
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1 


/««««««  file:  SERV34.FAS  ««»*««/ 


/««««««»»«««»««*»»»»«»««»««/ 
/«  PROCEDURE  JSERVO  «/ 
/««««»»««*«««««««««»««««««»/ 


/t: 

/« 

proorahmer: 

DENNIS 

PUGH 

*/ 

/« 

date: 

30-HAR- 

G2 

*/ 

/« 

function: 

ACCOMPLISHES  JACOBEAN  SERVO 

CONTROL . 

*/ 

/« 

ACTIVE 

COMPLIANCE  IS  TURNED 

ON  OR  OFF 

*/ 

/« 

BY  THE 

BOOLEAN  SNITCH  'SPRING* 

*/ 

/• 

»/ 

/% 

USER  guide: 

THE  CALLING  FORMAT  IS: 

%/ 

/* 

JSERVO ( 

MOVE  )l 

%/ 

f% 

*/ 

/* 

PROCEDURES 

called: 

KBADC.  RBDAC 

%/ 

/« 

*/ 

/* 

GLOBAL  VARIABLES 

«/ 

/* 

referenced: 

XRD»  YRO>  ZRD 

*/ 

/« 

XPD«  YPDt  ZPD 

•  / 

/t 

XFD.  YFD>  ZFD 

»/ 

/« 

ZEROFORCEi  SPRING 

*/ 

/« 

«/ 

/* 

modified: 

XPAt  YPA>  ZPA 

*/ 

/• 

XFA»  YFAt  ZPA 

*/ 

/* 

XFFt  YFF»  ZFF 

*/ 

/« 

PA8S1 

*/ 

/$ 

»/ 

PROCEDURE  JSERVO <  HOVEt  DOOLEAN  >r 


CONST 

CONPOAIN 

■ 

Ul 

• 

/* 

COMPENSATOR  GAIN 

•  / 

POAINX  > 
PGAINY  - 
POAINZ  - 

3.81 

3.81 

1. 321 

/* 

RECTILINEAR  POSITION  GAIN 

•  / 

FGAINX  - 
FGAINY  > 
FGAINZ  ■ 

O.OOOlt 

o.oooi: 

O.IASi 

/t 

RECTILINEAR  FORCE  GAIN 

*/ 

FILTPOLE 

-  2.0> 

/t 

POLE  FOR  FORCE  TERM  FILTER 

•  / 

PSCALE  - 

1.571» 

/% 

POSITION  SCALE  FACTOR 

*/ 

RSCALE  - 

o.6i: 

/* 

RATE  SCALE  FACTOR 

•/ 

ZFSCALE 

■  -I.2SI 

/* 

Z-AXIS  FORCE  SCALE  CHANGE 

«/ 

VSCALE  - 

10. Oi 

/% 

VOLTAGE  SCALE  FACTOR 

*/ 
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LI 

■ 

12.SA4S 

/* 

UPPER  LIND  LENGTH 

*/ 

L2 

m 

17.00S 

/* 

LOWER  LIND  LENGTH 

*/ 

L3 

m 

-1.43AS 

/« 

AZIHUTH  JOINT  OFFSET 

*/ 

L4 

m 

2.SS 

/* 

ELEVATION  JOINT  OFFSET 

$/ 

LS 

m 

2.43AS 

/« 

KNEE  JOINT  OFFSET 

«/ 

YHIP 

m 

8.SA2S 

/» 

Y  DISTANCE  FROH  HIP  TO  C.G. 

*/ 

VAR 


PSIi 

THETA* 

THETAl* 

THETA2> 

/t 

JOINT  ANGLES 

*/ 

CPSI> 

CTH* 

CTHlt 

CTH2> 

/t 

COSINES  OF  ANGLES 

*/ 

SPSIt 

STH* 

STHl* 

STH2> 

/* 

SINES  OF  ANGLES 

*/ 

TNlt 

TN2* 

Dl* 

02* 

/« 

INTERNEDIATE  TERNS 

«/ 

All> 

A12> 

/« 

INVERSE 

*/ 

A21> 

A22f 

A23> 

/t 

JACOBEAN 

t/ 

A31f 

A32f 

A33* 

/« 

NATRIX 

«/ 

XRC> 

YRC* 

ZRC* 

/t 

RECT.  RATE  CONNANDS 

*/ 

VOUTf 

/« 

HOTOR  OUTPUT  VOLTAGE 

*/ 

YSIGNt 

/* 

LEFT  SIDE  CORRECTION 

*/ 

HAXTINE 

1 

/« 

NAX  DT  FOR  FILTER 

*/ 

DTFi 

/t 

FILTER  DT 

%/ 

XFEf 

YFE* 

ZFE* 

/* 

RECT.  FORCE  ERROR 

*/ 

XFORCEi 

YFORCEt 

ZFORCE 

S  REALS 

/t 

FOOT  COORD.  FORCES 

*/ 

XHIP 

S  ARRAYAS 

/* 

X  COORD.  HIP  TO  C.G. 

*/ 

I  *  Jf 

/t 

LOOP  INDEX 

»/ 

CHANO*  CHANl* 

CHAN2 

S  INTEGERS 

/« 

A/D  CHANNEL  POINTERS 

»/ 

FORCE* 

/* 

FORCE  INPUT  ARRAY 

$/ 

POSITION* 

/* 

ACT.  JOINT  POSITIONS 

•  / 

RATE* 

/• 

ACTUAL  JOINT  RATES 

*/ 

RATECON* 

/* 

COHHAND  JOINT  RATES 

*/ 

VOLT 

/• 

HOTOR  VOLTAGE  OUTPUT 

•/ 

:  ARRAY 18 1 


PROCEDURE  RBAOC<  NOCHANt  FRSTCHS  INTEGER!  SCALES  REALS  VAR  INDATAS  ARRAY18  )S 
EXTERNALS 

PROCEDURE  RDDACS  NOCHANt  FRSTCHS  INTEGERS  SCALES  REALS  VOLTS  ARRAY18  )S 
EXTERNALS 

FUNCTION  SIGNS  XS  READS  REALS  EXTERNALS 
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BEGIN  /«  JSERVO  «/ 


XHIPCl]  i-  22.7St  XHIPC2]  t-  22.7St  XHIPC33  O.Oi 

XHIPC4]  O.Oi  XHIPCS3  -22.7Si  XHlPt63  :—22.7SI 

/*  CONDITION  OT  FOR  FAST  FILTER  B/ 

HAXTIHE  i-  1.0  /  (  FILTPOLE  «  4.0  >i 

IF  DT  >  HAXTIHE  <  THEN  FILTER  DEGENERATES  > 

THEN  DTF  !-  HAXTIHE 
ELSE  DTF  t>  DTI 

FOR  I  1  TO  A  DO 
DEOIN 

IF  I  IN  Cl>3fS3 

THEN  rSION  -1.0 
ELSE  YSION  1-  1.01 

CHANO  t-  <1-1>  «  3t 
CHANl  i-  CHANO  4  It 
CHAN2  t-  CHANO  4  21 

RBADC(3>  344CHAN0f  PSCALE.  POSITION) t  <  READ  JOINT  POSITION  > 
R0A0C<3>  CHANO*  FSCALE*  FORCE)  I  <  READ  FOOT  FORCES  } 

THETA2  i>  POSITIONCCHANOIt 
THETAl  t-  P0SITI0NCCHAN13I 
P8I  t-  P0SITI0NCCHAN23I 

/«  CONFUTE  TRUE  FORCES  IN  LEO  COORDINATES  «/ 

XFORCE  i>  F0RCECCHAN03  -  ZEROF0RCECCHANO3 I 

YFORCE  -<F0RCECCHAN13  -  ZEROFORCECCHANl 3 ) I 

ZFORCE  i>  ZFSCALE  «  (FORCEtCHAN23  -  ZEROFORCECCHAN23 ) I 

/•  CONFUTE  t  SAVE  ALL  NECESSARY  TRIO.  FUNCTIONS  »/ 

CPSI  COS<PSI)l 

SPSI  i-  SINCPSDI 
CTHl  :■  C08(THETA1>I 
STHl  }>  SIN( THETAl )» 

CTH2  t-  C08<THETA2)I 

8TH2  SIN<THETA2)I 

THETA  J-  THETA14THETA2I 

STH  :«  SIN<THETA>i 

CTH  C08<THETA)I 

TNI  i-  L1*CTH14LS<STH14L2»STHI 

D1  <■  L44TNlt 

D2  L2«(L1«CTH2-LS«STH2)I 
TN2  i-  TN1/D2I 

/«  CONFUTE  THE  ACTUAL  RECTILINEAR  FOOT  POSITION  4/ 

XPACI3  )•  OIBSPSI  -  L3tCPSI  4  XHIPCI3I 

YPACI3  i-  (DIBCPSI  4  L3BSP8I  4  YHIP)  «  YSIGNI 

ZPACIl  :>  -LlBSTHl  4  L2BCTH  4  LSBCTHII 


1 


/*  ROTATE  FORCES  TO  BODY 
XFACIT  i-  SFSI»CTH 

-  YSIONBCPSI 
f  SPS1B8TH 


COORDINATES  «/ 
«  XFORCE 
«  YFORCE 
»  ZFORCEt 


YFACIl  YS1QN«CPSI«CTH 
f  STH 

*  YS1QN«CPSI«STH 


«  XFORCE 
«  YFORCE 
»  ZFORCE» 


ZFACIT  -STH 
+  CTH 


*  XFORCE 
«  ZFORCEt 


IF  HOVE  -  TRUE  THEN  <  SERVO  THE  HEXAPOD  > 
BEGIN 


IF  SPRING  >  FALSE  <  THE  FORCE  TERH  HU8T  BE  ZEROED  > 

THEN 

BEGIN 

xFFcn  o.o( 

YFFCn  S-  0.0» 

ZFFCII  0.0» 

END 

ELSE 

IF  PA8S1  •>  TRUE  <  THE  FILTER  NUST  BE  INITIALIZED  > 
THEN 

BEGIN 

XFFCIT  O.OI 
YFFCI3  t-  0.0» 

ZFFtI3  i"  0.01 
END 

ELSE 

BEGIN 

/«  CONFUTE  THE  FORCE  ERRORS  */ 

XFE  J-  XFDtl3  -  XFACI3» 

YFE  J-  YFDtl3  -  YFAtI3» 

ZFE  t«  ZFDCI3  -  ZFAtI3» 


/«  LIHIT  FORCE  ERRORS  TO  GIVE  4'  HAX.  DEFLECTION  */ 
IF  ABStXFE)  >  4.0  «  POAINX/FGAINX 

THEN  XFE  :■  4.0  t  PQAINX/FOAINX  *  S1GN(  XFE  )l 


IF  ABSIYFE)  >  4.0  »  POAINY/FGAINY 

THEN  YFE  t-  4.0  t  PGAINY/FGAINY  *  SIGNC  YFE  >1 

IF  ABS(ZFE)  >  4.0  «  POAINZ/FOAINZ 

THEN  ZFE  t-  4.0  *  PGAINZ/FGAINZ  «  SION(  ZFE  )l 


/«  LOU-PASS  FILTER  THE  FORCE  ERRORS  «/ 

XFFtI3  I-  XFFtI3  ♦  FILTPOLE  *  (XFE  -  XFFCI3>  «  DTF » 
YFFCI3  :■  YFFCI3  ♦  FILTPOLE  «  (YFE  -  YFFtI3>  *  DTF» 
ZFFCI3  {•  ZFFCI3  ♦  FILTPOLE  «  (ZFE  -  ZFFCI3)  *  DTFI 
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END  I 


<  ELSE  > 


{  END  IF  PA8S1  > 

<  END  ELSE  > 

<  END  IF  SPRING  > 

/«  CONPUTE  RECTILINEAR  FOOT  RATE  CONNAND  «/ 

XRC  XRDCII  >  PQAINX  «  (XPDCI]  -  XPACIT)  *  FGAINX  «  XFFCITi 

YRC  :>  (YROCn  *■  POAINY  «  <YPDCIT  -  YPACll)  »  FOAINV  t  YFFCII)  «  YSIONI 

ZRC  t-  ZRDCII  *  PGAINZ  •  <ZPOCI3  -  ZPAC13)  i  FGAINZ  «  ZFFCIll 

/*  COMPUTE  INVERSE  JACODIAN  MATRIX  «/ 

All  S«  CPSI/Dll 
A12  :■  -SPSI/Oll 

A21  :>  -<L2«SPSI«STH)/D24L2«L3«STHtCPSI/Dl/D2t 

A22  :«  -L2«STH«CPSI/02-L2«L3«STH«SPSI/Dl/D2t 

A23  !■  -L2«CTH/D2I 

A31  :«  (SPS1'L3«CPS1/01>«TN2I 

A32  (CPSUL3*SPSI/Dl)tTN2t 

A33  t-  (-L1«STH1FL2*CTH'FL5«CTH1)/D2I 

/«  COMPUTE  JOINT  RATE  CONNAND  */ 

RATEC0HCCHAN2]  AlltXRC  t  A12*YRC> 

RATECOHCCHANll  i-  A21tXRC  f  A22tYRC  F  A23»ZRC> 

RATECOMCCHANOI  A31FXRC  f  A32aYRC  F  A33«ZRC» 


/«««  RATE  SERVO  SECTION  *««/ 

RDA0C(3fl8FCHAN0fRSCALEtRATE)»  <  FETCH  ACTUAL  JOINT  RATES  > 

FOR  J  :■  CHANO  TO  CHAN2  DO 
DEOIN 

VOUT  COHPGAIN  «  (RATECOMCJT  -  RATECJ3>> 

IF  VOUT  >  9.8  THEN  VOUT  :»  9.8 

ELSE  IF  VOUT  <  -9.8  THEN  VOUT  -9.8» 

VOLTCJ3  ;■  VOUTf 

RBDACdt JiVSCALE«VOLT)»  <  OUTPUT  THE  VOLTAGE  > 

ENOI  <  DO  > 

END  I  {  IF  MOVE  > 

ENOI  <  DO  > 

PASSl  FALSEI  <  FLAG  THAT  FILTERS  ARE  INITIALIZED  > 

ENDI  <  JSERVO  > 
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(••Ei*) 


FILE!  1NIT35.PAS  ««»»««$/ 


/«  PROCEDURE!  NORNALIZE  «/ 


/*! 

itt******] 

/t 

PROGRANNER! 

DENNIS 

PUOH 

*/ 

/• 

DATE! 

30-NAR- 

'82 

*/ 

/« 

FUNCTION! 

HOVE  HEXAPOD  TO 

NORNALIZED  POSITION 

*/ 

/• 

USER  GUIDE! 

THE  CALLING  FORHAT  IS! 

%/ 

/* 

NORNALIZE! 

%/ 

/« 

*/ 

/• 

*/ 

/« 

PROCEDURES 

CALLED! 

JSERVOt 

HALTi  FOOTLINE 

*/ 

/t 

TURNON t 

RBADC 

*/ 

/t 

*/ 

/* 

GLOBAL  VARIABLES 

*/ 

/* 

REFERENCED! 

XPAt 

YPAt  ZPA 

«/ 

/* 

MIDSTXt 

NIDSTYt  HIDSTZ 

*/ 

/* 

*/ 

/• 

HOOIFIED! 

ZEROFORCEt  FZERO 

»/ 

/• 

%/ 

/t*t*tt***ttitt*t****ttt**t****t»t***»*t*ttt***t****t*t*t****ttttt/ 

PROCEDURE  NORNALIZEI 


VAR  FOOT  !  INTEGER!  <  FOOT  INDEX  > 

FRSTCM  !  INTEGER!  <  FIRST  A/0  CHANNEL  > 

XCOORDt  YCOORDf  ZCOORD  !  ARRAY6!  <  FOOT  COORDINATES  > 


PROCEDURE  J8ERVO(  HOVE!  BOOLEAN  )l  EXTERNAL! 

PROCEDURE  FOOTLINEt  VELNAX!  REAL!  XCOORDt  YCODRDr  ZCOORD!  ARRAYA  )! 

EXTERNAL! 

PROCEDURE  RBADC<  NOCHANt  FRSTCH!  INTEGER!  SCALE!  REAL!  VAR  INDATA!  ARRAY18  )! 
EXTERNAL! 

PROCEDURE  HALT!  EXTERNAL! 

PROCEDURE  TURNON!  EXTERNAL! 
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BEGIN 

URITELNt 

URITELN<  CHR(27)t  '*6  NORHALIZING 

JSERVO<  NONOVE  >1  <  FETCH  ACTUAL  FOOT  POSITIONS  > 

TURNONt  <  TURN  ON  NOTOR  POWER  > 

FOR  FOOT  t>  1  TO  A  DO  <  INITIALIZE  COORDINATES  > 

BEGIN 

XCOORDCFOOTI  XPACFOOTIi 
YCOORDCFOOTI  t-  VPACFOOT3*. 

ZCOOftOCFOOTl  t-  HIBSTZ» 

END! 

FOOTLINE(  3. Of  XCOOROf  VCOORDf  ZCOORD  >>  <  HOVE  TO  HIDSTANCE  Z  > 


FOR  FOOT  {-  1  TO  A  DO 

IF  FOOT  IN  C  If  4f  S  I 

THEN  ZCOORDCFOOTl  HIDSTZ  -  S.OI 
FOOTLINE(  3. Of  XCOORDt  YCOORDf  ZCOORD  )t  <  HOVE  LEG  SET  1  UP  > 

FOR  FOOT  1  TO  A  DO 

IF  FOOT  IN  C  If  4t  S  I  THEN 
BEGIN 

XCOORDCFOOT3  tn  NIDSTXCFOOT] t 
YCOORDCFOOTI  NIDSTYCFOOT 3  I 
END  I 

FOOTLINE<  3. Of  XCOORDf  YCOORDf  ZCOORD  )l  <  HOVE  LEO  SET  1  OVER  > 

FOR  FOOT  1  TO  A  DO 

IF  FOOT  IN  C  If  4f  3  3  THEN 
BEGIN 

FRSTCH  }-  <  FOOT  -  I  >  *  3»  <  CONFUTE  FIRST  A/D  CHANNEL  > 

RBADC<  3f  TRSTCHf  FSCALEf  ZEROFORCE  )«  <  READ  FORCE  OFFSET  > 
FZEROCFOOT3  t-  TRUE>  <  FLAG  THAT  ZEROFORCE  UPDATED  > 

ZCOORDCFOOTl  S-  NIDSTZI 
END! 

FOOTLINE(  3. Of  XCOORDf  YCOORDf  ZCOORD  )l  <  HOVE  LED  SET  1  DOWN  > 

FOR  FOOT  1  TO  A  DO 

IF  FOOT  IN  C  2f  3f  A  3 

THEN  ZCOORDCFOOTl  HIDSTZ  -  S.OT 
FOOTLINEC  3. Of  XCOORDf  YCOORDf  ZCOORD  >1  <  HOVE  LEG  SET  2  UP  > 

FOOTLIN£(  5. Of  HIDSTXf  HIDSTYf  ZCOORD  >1  <  HOVE  LEG  SET  2  OVER  > 
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END! 


FOR  FOOT  1  TO  6  DO 

IF  FOOT  IN  C  2>  3t  6  ]  THEN 
BEGIN 

FRSTCH  :>  (  FOOT  -  1  >  «  3(  <  COMPUTE  FIRST  A/D  CHANNEL  > 

RBAOC<  3>  FRSTCH*  FSCALE*  ZEROFORCE  )>  <  READ  FORCE  OFFSET  > 
FZEROCFOOTI  :>  TRUE)  <  FLAG  THAT  ZEROFORCE  UPDATED  > 

ZCOOROCFOOTT  HIOSTZt 
END! 

FOOTLINE<  5.0*  HIOSTX*  HIDSTY*  ZCOORD  >)  {  HOVE  LEG  SET  2  DOWN  > 

HALT) 

URITELNt 

URITELN(  CHR(27)>  '  NORMALIZATION  COMPLETE' >t 
URITELNI 

/«  NORMALIZE  «/ 
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/«««»«««««««««»*««»«»*»»««»*»**/ 

/*  PRoceouRc:  initialize  »/ 


rr 


I 

t 


/* 

PROGRANNERt 

DENNIS 

PUGH 

*/ 

/* 

date: 

20-APR 

-82 

%/ 

/* 

funcion: 

RAISE 

LEGS  TO  WALKING  POSITION 

%/ 

/* 

*/ 

/t 

USER  GUIDE! 

THE  CALLING  FORMAT  IS! 

*/ 

/* 

INITIALIZE! 

»/ 

/» 

*/ 

/* 

*/ 

/* 

PROCEDURES 

called: 

FOOTLINEf  HALT>  TURNON 

*/ 

/* 

%/ 

/* 

GLOBAL  VARIABLES 

%/ 

/* 

referenced: 

BETA>  RPHASE 

*/ 

/* 

*/ 

/* 

modified: 

PHASE t  SUPPORT#  ZPTERM 

*/ 

/* 

*/ 

PROCEDURE  INITIALIZE! 

VAR  FOOT  :  INTEGER i 

TPHASE  t  REAL! 

ZINIT  :  ARRAY6! 

PROCEDURE  HALT!  EXTERNAL! 

PROCEDURE  FOOTLlNE(  VELHAX:  REAL!  XCOORDi  YCOORD>  ZCOORDS  ARRAY6  >!  EXTERNAL! 
PROCEDURE  TURNON!  EXTERNAL! 


BEGIN 

NRITF'.N! 

URITEi.N<  CHR<27)f  '*6  INITIALIZING  ...'>! 

SUPPORT  :>  C  t..6  I!  <  START  WITH  ALL  FEET  IN  SUPPORT  PHASE  > 
TURNON!  <  TURN  ON  NOTOR  POWER  > 
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I 


i 


/ttt  COHPUTE  DESIRED  INITIAL  FOOT  HEIGHTS  «««/ 

FOR  FOOT  1  TO  6  DO 
BEGIN 

ZPTERHC  FOOT  3  <-  O.Ot  <  INITIALIZE  ATTITUDE  CORRECTION  TERN  > 

IF  RPHASEC  FOOT  3  <  BETA 
THEN 

ZINITC  FOOT  3  HIDSTZ 

ELSE 

BEGIN 

SUPPORT  SUPPORT  -  CFOOT3>  <  REHOVE  FOOT  FROH  SUPPORT  SET  > 
TPHASE  <  RPHASEC  FOOT  3  -  BETA  >  /  (  1.0  -  BETA  >» 

ZINITC  FOOT  3  :>  HIDSTZ  -  FOOTLIFT  «  SIN(  TPHASE  »  PI  >i 
END* 

<  END  IF  > 

ENDI  <  FOR  FOOT  > 

FOOTLINE<  S.Of  HIDSTXt  HIOSTYt  ZINIT  )l  C  RAISE  LEGS  IN  TRANSFER  PHASE  > 
HALT! 

PHASE  %•  0.01  <  INITIALIZE  KINEHATIC  CYCLE  PHASE  > 

HRITELNi 

URITELN<  CHR<27)f  '00  INITIALIZATION  COHPLETE ' > t 
HRITELNI 

ENDI  /«  INITIALIZE  «/ 
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/««««»««•««$«»««»«$»»«$/ 

/«  PROCEDURE  UPDOUN  «/ 

/t****»t**tt*tt***t»**t/ 


/«»#««««««««*»««««««»««»«»»«»»»«»««»»««»*»«»««««»«»««»«*»«»»»»»»»/ 


/«  proorahher:  dennis  pugh  «/ 

/*  date:  20-APR-82  «/ 

/«  function:  change  the  body  elevation  «/ 

/*  «/ 

/«  USER  guide:  the  calling  forhat  is:  updoun:  height  )i  */ 

/«  WHERE  ‘HEIGHT*  IS  THE  DESIRED  BODY  ELEVATION  «/ 

/#  */ 

/*  »/ 

/«  PROCEDURES  CALLED:  FOOTLINEt  HALT.  TURNON  «/ 

/•  */ 

/t  GLOBAL  VARIABLES  */ 

/«  referenced:  hidstx.  hidsty  */ 

/t  */ 

/«  HOOIFIEO:  NONE  t/ 

/*  */ 

PROCEDURE  UPD0HN<  HEIGHT  :  REAL  )» 

VAR  FOOT  :  INTEGER!  /«  FOOT  INDEX  */ 

ZCOORO  :  ARRAY6!  /*  FOOT  Z  COORDINATES  9/ 


PROCEDURE  FOOTLINE(  VELHAX:  REAL!  XCOORD.  YCOORD.  ZCOORD:  ARRAY6  >1 
EXTERNAL! 

PROCEDURE  HALT!  EXTERNAL! 

PROCEDURE  TURNON!  EXTERNAL! 

BEGIN 

TURNON!  <  TURN  ON  NOTOR  POWER  > 

FOR  FOOT  :-  1  TO  4  DO  ZCOORDCFOOT]  :-  HEIGHT! 

FOOTLINE!  3.0.  HIDSTX.  HIDSTY.  ZCOORD  >!  <  HOVE  TO  DESIRED  ELEVATION  > 

HALT! 

END!  <  UPDOWN  > 
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(•tEt*) 


/«««$«*«  FILE)  L1NE34.PA8  ««««$««/ 


/«  PROCEDURES  FOOTLINE  «/ 

/««««««««*»«»«««»«««««»«»»»«»/ 


/«  PROORRNNERi  DENNIS  PUGH  «/ 
/t  DATES  20-APR-82  «/ 
/*  «/ 
/«  FUNCTIONS  HOVE  FEET  FROM  PRESENT  POSITIONS  TO  */ 
/«  SPECIFIED  POSITIONS  IN  STRAIGHT  LINES  «/ 
/*  WITH  ALL  NOTIONS  COHPLETED  SIHULTANEOUSLY  */ 
/*  */ 
/«  USER  GUIDES  THE  CALLING  FORNAT  ISS  */ 
/*  FOOTLINES  UELNAXt  XCOORDt  YCOORDt  ZCOORO  «/ 


/*  «/ 


/t 

PROCEDURES  CALLED! 

JSERVO 

*/ 

/* 

%/ 

/* 

GLOBAL  VARIABLES 

%/ 

/* 

REFERENCED! 

XPAt 

YPAf 

ZPA 

«/ 

/■ 

»/ 

/* 

MODIFIED! 

XPDt 

YPDf 

ZPD 

*/ 

/* 

XROf 

YRDf 

ZRD 

«/ 

/« 

DTt 

SPRING 

%/ 

/t 

%/ 

PROCEDURE  FOOTLINES  VELMAXS  REAL*  XCOORDi  YCOORDi  ZCOORDS  ARRAYS  )S 


CONST  GAIN  -  2.01  /* 

VAR  ERRORXt  ERRORYi  ERRORZ  /* 

S  ARRAYSI 

PTIHEf  LTIMEf  /* 

NAXERR  /« 

S  REALS 

FOOT  /* 

:  INTEGERS 


PROCEDURE  JSERVOS  NOVES  BOOLEAN  )S 


POSITION  ERROR  TO  RATE  GAIN  »/ 

POSITION  ERROR  FROH  SETPOINT  «/ 

TEHPORARY  STORAGE  FOR  TINE  «/ 
NAXINUN  OF  COORDINATE  ERRORS  »/ 

FOOT  INDEX  «/ 

EXTERNALS 
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BEGIN 

SPRING  FALSE!  <  DISABLE  ACTIVE  CONPLIANCE  > 

JSERVO!  NOHOVE  )l  <  FETCH  ACTUAL  POSITIONS  > 

FOR  FOOT  t-  1  TO  A  DO.  <  INITIALIZE  DESIRED  POSITIONS  > 

BEGIN 

XPDCFOOTT  t-  XPACFOOT3I 
YPOCFOOTl  YPACFOOTII 
ZPDCFOOTl  t-  ZPACFOOTl! 

END! 

LTIHE  TINE! 

DT  :■  0.01! 

REPEAT 

HAXERR  :«  O.Oi 

FOR  FOOT  :>  1  TO  A  DO 
BEGIN 

ERRORXCFOOTI  !-  XCOORDCFOOTI  -  XPAIFOOTI! 

ERR0RYCF00T3  :»  YCOORDCFOOT3  -  YPACF00T3! 

ERRORZCFOOTI  t-  ZCOORDtFOOT]  -  ZPACFOOTl! 

IF  ABS(  ERRORXCFOOT]  )  >  HAXERR  THEN  HAXERR  !-  ABS(  ERRORXCFOOTI  >! 

IF  ABS(  ERRORYCFOOT]  )  >  HAXERR  THEN  HAXERR  !-  ABS(  ERRORYEFOOT]  >! 

IF  ABS<  ERRORZCFOOT]  )  >  HAXERR  THEN  HAXERR  !>  ABS(  ERRORZCFOOT]  >! 

END!  /*  FOR  FOOT  */ 


FOR  FOOT  !>  1  TO  A  DO 
BEGIN 

IF  (  GAIN  «  HAXERR 
THEN  BEGIN 

XRDCFOOT] 

YR0CF00T3 

ZRDCF00T3 

END 

ELSE  BEGIN 

XRDCFOOT] 

YRDCFOOT] 

ZRDCFOOT] 

END! 


)  <  VELHAX  <  INCHES  PER  SECOND  > 


!■ 

GAIN 

* 

ERRORXCFDOT]! 

!■ 

GAIN 

« 

ERRORYCFOOT]! 

:■ 

GAIN 

« 

ERRORZCFOOT]! 

:■ 

VELHAX 

« 

ERRORXCFOOT] 

/ 

HAXERR! 

VELHAX 

« 

ERRORYCFOOT] 

/ 

HAXERR! 

VELHAX 

« 

ERRORZCFOOT] 

f 

HAXERR! 

XPDCFOOT]  !-  XPDCFOOT]  A  XRDCFOOT]  *  DT! 
YPDCFOOT]  YPDCFOOT]  ♦  YRDCFOOT]  «  DT! 
ZPDCFOOT]  ZPDCFOOT]  A  ZRDCFOOT]  «  DT! 


END!  /*  FOR  FOOT  «/ 


JSERVOi  HOVE  >! 
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END  I 


PTIHE  I-  TIHEI 

OT  t-  <PT1NC  -  LTIHE)  «  3600.01 

UTIME  ;»  PTIMEI 

UNTIL  HAXERR  <  O.SI 

FOR  FOOT  t-  1  TO  6  DO  <  CLEAN  UP  GLOBAL  VARIABLES  > 
BEGIN 

XROCFOOT]  0.01 
YRDCFOOTl  i-  0.0> 

ZR0CF00T3  0.01 

XPDCF00T3  :>  XCOORDCFOOTIt 
YPDCF00T3;-  YC00RDCF00T3I 
ZP0CF00T3  ZC00RDCF00T3> 

END* 

/«  FOOTLINE  »/ 
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3 


/«  PROCEDURE t  HALT  */ 


/* 

/« 

/« 

/* 

/« 

/* 

/« 

/« 

/t 

/« 

/• 

/* 

/* 

/* 

/« 


PROORAHHERi  DENNIS  PUSH 
date:  20-APR-a2 

function:  stops  all  notion  of  hexapod 

USER  guide:  the  calling  fornat  is: 

HALT! 


procedures  called: 


RBDAC 


GLOBAL  VARIABLES 

referenced: 

hooified: 


NONE 


VELXf 

NVELX* 


VELY> 

NVELYi 


DPS  I 
NDPSI 


*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

%f 

%/ 

%/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 


PROCEDURE  HALT  I 


CONST  VSCALE  ■  10. Ot 

VAR  JOINT  :INTE0CRI  /«  JOINT  INDEX  «/ 

VOLT  :ARRAYI8»  /*  OUTPUT  VOLTAGES  »/ 

PROCEDURE  RBDAC<  NOCHAN>  FRSTCH:  INTEGER!  SCALE:  REAL!  VOLT:  ARRAYIB  >! 
EXTERNAL! 


PROCEDURE  TURNOF!  EXTERNAL! 

BEGIN 

FOR  joint:-  0  TO  17  DO 
VOLTCJOINTI  :-0.0! 

RBDAC!  IBt  Ot  VSCALEt  VOLT)!  /*  TURN  OFF  VOLTAGE  TO  ALL  NOTORS  «/ 
TURNOF!  /«  TURN  OFF  NOTOR  POWER  */ 


/«««  INITIALIZE  VELOCITY  VECTORS  a««/ 

NVELX  :-  0.0!  NVELY  :-  0.0!  NDPSI  :-  0.0! 

VELX  :-  0.0!  VELY  !-  0.0!  DPSI  :-  0.0! 

URITELN! 

WRITELN!  CHR<27)t' BO— HEXAPOD  STOPPED—')! 


END!  /«  HALT  «/ 


1 


I 

I 


\ 
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r. 


<««E+«) 


file:  LIBR34.PA8  **«t*«/ 


/««*«««•««»*«»««»•»»$«««»/ 

/«  function:  ATAN2  «/ 


/«  prooranner:  dennis  puon  «/ 

/«  date:  20-APR-82.  »/ 

/«  function:  inplehent  the  four  -  quadrant  %/ 

/*  INVERSE  TANGENT  FUNCTION  «/ 

*/ 

/t  USER  guide:  the  calling  fornat  is:  »/ 

/«  ATAN2<Y>  X>l  «/ 

/•  where:  y  is  the  side  opposed  to  the  angle  «/ 

/*  X  IS  THE  side  adjacent  TO  THE  ANGLE  «/ 

*/ 


FUNCTION  ATAN2<Yi  X  :  REAL)  :  REAL) 


FUNCTION  sign:  x:  real  >:  read  external) 


BEGIN 

IF  AB8<X)  >  0.00001 

THEN  <  X  IS  NON-ZERO  > 

IF  X  >  0.0 
THEN 

ATAM2  :•  ARCTANIY/^) 

ELSE 

ATAN2  :-  ARCTAN(Y/X>  )>  PI  >  SIGN(  Y  > 
<  END  IF  X  > 

ELSE 

ATAN2  :*  PI  /  2.0  «  sign:  Y  >) 

END)  /«  ATAN2  «/ 
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/«  function:  SION  */ 

/««««•««««*««««»«»««$«**««««»««»«»»**$»««««*««»««*«*«*«»«««««««««/ 


/* 

PROQRANHERi 

DENNIS  PUGH 

•  / 

/« 

date: 

20-APR-82 

*/ 

/t 

function: 

IHPLEHENT  THE  SIGNUN  FUNCTION 

*/ 

/« 

*/ 

/« 

USER  guide: 

THE  CALLING  FORHAT  IS: 

«/ 

/* 

sign:  X  )) 

*/ 

/* 

$/ 

FUNCTION  S1QN<  X  :  REAL  )  :  REAL! 
BEQIN 

IF  X  >■  0.0 
THEN  SION 
ELSE  SION 

ENOt 


t-  1.0 
-1.0 
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(««E4«) 


/««««««  FILCt  DLNK34.PAS  «•«»«*/ 


/«««««««««»»««»«««*«*««*/ 
/*  PROCEDURES  RDADC  «/ 
/««$««««««»««««««««««•»«/ 


/«  proqranher:  dennis  puoh  «/ 
/«  date:  20-APR-82  «/ 
/«  FUNCTIONS  READ  IN  INPUT  VARIABLES:  FORCEt  RATEt  t  «/ 
/«  POSITION.  «/ 
/*  »/ 
/«  USER  GUIDE :  THE  CALLING  FORHAT  ISS  «/ 
/«  RBADC(  NOCHAN.  FRSTCH.  SCALE.  INDATA  >1  •/ 
/«  WHERE :  «/ 
/«  NOCHAN  IS  THE  NUHBER  OF  CHANNELS  TO  BE  READ  «/ 
/«  FRSTCH  IS  THE  FIRST  CHANNEL  TO  BE  READS  %/ 
/%  0  FOR  READING  FORCE  VARIABLES  */ 
/«  IS  FOR  READING  JOINT  RATES  */ 
/B  3A  FOR  READING  JOINT  ANGLES  */ 
/«  SCALE  IS  THE  INPUT  SCALING  FACTOR  «/ 
/*  INOATA  IS  THE  INPUT  BUFFER  «/ 
/*  */ 
/*  */ 
/«  PROCEDURES  CALLED:  NONE  «/ 
/*  */ 
/t  GLOBAL  variables:  NONE  */ 
/*  */ 


i 

I 

I 

I 


Procedure  rbaoc(nochan.  frstchs  integers  scale:  reals  var  indata:  arravigss 

VAR  l.  J  /«  DATA  POINTERS  «/ 

S  INTEGERS 

AOPORT  ORIGIN  16A000B  /«  LOCATION  OF  DATA  LINK  SHARED  HENORY  «/ 

:  ARRAY!  0..&3  3  OF  INTEGERS 

BEGIN  /«  RBAOC  */ 

J  :-  FRSTCH  HOD  18S  /«  OFFSET  FROH  START  OF  VARIABLE  TYPE  */ 

FOR  I  :-  J  TO  (J  F  NOCHAN  -  1 >  DO  <  CONVERT  (  SCALE  DATA  > 

INDATACI3  <777B  -  ADPORTC  FRSTCH-J  +13)/  777B  »  SCALES 
ENDS  /«  RBADC  «/ 
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/«  PROCEDURES  RBDAC  «/ 

/«$««««««««•«$«»•«»••«««/ 


/«  prograhher:  dennis  puoh  »/ 
/«  date:  20-APR-82  «/ 
/«  FUNCTIONS  SEND  OUTPUT  VOLTAGES  TO  DATA  LINK  «/ 
/*  */ 
/«  USER  GUIDES  THE  CALLING  FORMAT  ISS  «/ 
/«  RBDACt  NOCHANt  FRSTCHr  SCALE*  OUTDATA  )  %/ 

/«  where:  «/ 

/«  NOCHAN  IS  THE  NUMBER  OF  OUTPUT  CHANNELS  «/ 
/*  FRSTCH  IS  THE  FIRST  CHANNEL  TO  BE  OUTPUT  «/ 
/*  SCALE  IS  THE  OUTPUT  SCALING  FACTOR  */ 
/«  OUTDATA  IS  THE  OUTPUT  BUFFER  */ 
/t  */ 
/*  »/ 
/«  PROCEDURES  CALLED:  NONE  «/ 
/«  «/ 
/*  GLOBAL  variables:  NONE  */ 
/«  «/ 


PROCEDURE  RBDAC(NOCHAN  tFRSTCHS  INTEGER*  SCALES  REAL*  OUTDATA*  ARRAYIS)* 


CONST  READY  -  200B* 

VAR  It 

TEMP 

:  INTEGER* 

STATUS  ORIGIN  16A24AB 
:  INTEGER* 

OUTPORT  ORIGIN  1AA200B 
:  ARRAYC0..173 


/* 

DATA 

POINTER 

*/ 

/* 

TEMPORARY 

STORAGE 

FOR  OUTPUT 

*/ 

/*'  DATA 

LINK 

STATUS 

WORD 

*/ 

/* 

DATA 

LINK 

OUTPUT 

PORTS 

*/ 

INTEGER* 


BEGIN  /*  RBDAC  */ 


FOR  I  S-  FRSTCH  TO  (FRSTCH  ♦  NOCHAN  -  I >  DO 
BEGIN 

TEMP  177B  -  ROUNDS OUTDAT AC n  «  177B  /  SCALE)* 

WHILE  (STATUS  AND  READY)  -  0  DO  <  WAIT  UNTIL  DATA  LINK  READY  >* 
OUTPORTCI3  S-  TEMP* 

END*  /«  DO  «/ 

END*  /«  RBDAC  «/ 
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PROCEDURE  CLOCKINITt 

VAR  STATUS  ORIGIN  170404B  S  INTEGER) 

PRELOAD  ORIGIN  17040AB  :  INTEGER) 

CLOCK  ORIGIN  170414B  :  INTEGER) 

BEGIN 

PRELOAD  0)  <  USE  FULL  COUNTER  INTERVAL  > 

STATUS  413B)  <  ENABLE  COUNTER  AT  100  Hz  > 

LASTCLOCK  t-  CLOCK) 

totalclock  :>  0) 

END) 


FUNCTION  DELTATINE  :  REAL) 

VAR  CLOCK  ORIGIN  170416D  :  INTEGER) 

NOUCLOCK  :  INTEGER) 

NUNTICKS  :  INTEGER) 

BEGIN 

NOUCLOCK  CLOCK) 

NUNTICKS  S»  <  NOUCLOCK  -  LASTCLOCK  )  AND  377B) 
TOTALCLOCK  1-  TOTALCLOCK  4  NUNTICKS) 

LASTCLOCK  NOUCLOCK) 


DELTATINE  .*>  NUNTICKS  /  100.0) 


(««£+«) 


/««««««  file:  P0UR34.PAS  ««*•»*/ 

/«»«««««««««»««««««««»«««««««»*•«»««*«*«**»*«*««*«««««»**«**$«»««**»«**««/ 


/*  %/ 
/«  proorahher:  oennis  puoh  »/ 

/«  date:  20-APR-82  «/ 
/*  «/ 
/«  GLOBAL  GARIABLESt  NONE  «/ 
/*  */ 
/«  THIS  FILE  CONTAINS  SUBROUTINES  WHICH  UTILIZE  THE  SELF-DIAGNOSTIC  «/ 
/«  CAPABILITIES  AND  THE  REHOTE  POWER  CONTROL  CAPABILITIES  OF  THE  »/ 
/«  DIGITAL  DATA  LINK.  THE  EXTERNALLY  USEFUL  ROUTINES  ARE  ‘TESTON‘>  */ 
/«  *TURNOF*f  AND  ‘TURNON*.  NONE  OF  THESE  REQUIRE  ARGUHENTS.  «/ 
/*  */ 


PROCEDURE  PWAITI  /*  A  ROUTINE  WHICH  WAITS  UNTIL  ALL  FEEDBACK  CHANNELS  «/ 
/«  HAVE  BEEN  UPDATED  AFTER  A  CHANGE  IN  STATUS  t/ 

VAR  now: real: 

BEGIN 

nqh:-tihei 

WHILE  ((TINE-N0H)f3600.>  <  0.004  <*  SECONDS  «>  DO  <«  NOTHING  *>  » 

END*  /«  PWAIT  «/ 
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/««**»»«*»««»$««»«««*««$»«»««*««»»«»««»«»»»»»»»»*»$«»««*«*«»*«»»*»«»«»»«*/ 


/*  */ 
/*  turmof:  */ 

/«  (TURN  OFF)  IS  A  ROUTINE  WHICH  TURNS  OFF  THE  FOUER  TO  THE  «/ 
/«  SERVO  HOTORS.  IT  SHOULD  BE  USED  BEFORE  EXITING  THE  CONTROL  «/ 
/«  PROORANt  AND  ADDITIONALLY,  IT  SHOULD  BE  CALLED  WHENEVER  THE  */ 
/«  HEXAPOD  IS  NOT  ACTUALLY  SERVOINO.  THIS  WILL  INCREASE  SAFETY  t/ 
/«  AND  REDUCE  THE  RISK  OF  DAMAGE  TO  THE  HEXAPOD.  THE  EFFECTS  «/ 
/«  OF  CALLING  TURNOF  CAN  BE  REVERSED  BY  CALLING  TURNON.  $/ 
/*  */ 


PROCEDURE  TURNOF I 


(««C 

CONSTAT 

POWER-" 

•"0166244 

0166246 

iADDRESS  OF  COMMAND  t  FORWARD  STATUS 
.ADDRESS  OF  POWER  COMMAND  WORD 

t: 

«) 

.MACRO 

TSTB 

BPL 

MOV 

.ENOM 

SEND  Y.ZtTT  ’ 
CONSTAT 

T 

Y.Z 

;CHECK  IF  FORWARD  PATH  BUSY 

)SEND  DATA 

BEGIN 

(««C 

S) 

SEND 

SEND 

«OiCOHSTAT 

•2>P0UER 

ICONMAND  NORMAL  MODE 

ITURN  OFF  MOTOR  POWER 

ENDI 

/«  PROCEDURE  TURNOF  t/ 
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PROCEDURE  ASKi  /«  A  PROCEDURE  WHICH  GIVES  THE  OPTION  TO  ABORT  */ 

(««C  .HCALL  EXITBS  «> 

VAR  letter: CHAR t 
BEGIN 

UR1TELN<'D0  YOU  WISH  TO  ABORT?  (Y/N>'>» 

READLN(LETTER) i 
IF  LETTER  •  'Y'  THEN 
BEGIN 

uriteln: 

WRITELN:'  «»*  PROGRAM  ABORTED  *««')< 

TURNOFt 

(*»C  EXITtS  «)  /«  EXIT  THE  PROGRAM  */ 

ENOi 

END>  /«  PROCEDURE  ASK  «/ 

/ttttttt**t**t*****«t*******9**9***t*M*$******t*****tt**tttt*t***tt*ttttt/ 


/tttt******n*t*****t**ttt********$*********t*tt*tt*t******t***tt*t**t*t**/ 


/*  */ 

/*  turnon: 

/«  (TURN  ON)  IS  A  ROUTINE  WHICH  TURNS  ON  THE  HEXAPOD  MOTOR  t  */ 

/*  ELECTRONICS  POWER.  IT  ASSUMES  THAT  'TESTON*  HAS  BEEN  CALLED  */ 

/*  SUCCESSFULLY.  IT  SHOULD  BE  CALLED  AT  THE  START  OF  EVERY  »/ 

/*  REAL  TIME  SECTION  (WHEN  THE  HEXAPOD  IS  ACTIVELY  SERVOING).  »/ 

/*  »/ 


PROCEDURE  TURNON) 

(««C  CaMSTAT°''0166244  (ADDRESS  OF  COMMAND  t  STATUS  WORDS 

P0WER>''0I6«246  (ADDRESS  OF  POWER  COMMAND  WORD 

PSTAT°~01A6176  (ADDRESS  OF  POWER  STATUS  WORD 

.MACRO  SEND  Y.ZrTT 

T:  TSTB  CONSTAT  (CHECK  IF  FORWARD  PATH  BUSY 

BPL  T 

MOV  Y.Z 

.ENDM 


(SEND  DATA 


BEGIN 


(«tC 

«) 


BIT 

BEQ 


•4tPSTAT 

KIL 


(TEST  KILL  CIRCUIT  STATUS 
IBRANCH  IF  INACTIVE 


URITELNi 

URITELN<'THE  KILL  CIRCUIT  IS  ACTIVE*  PLEASE  DEACTIVATE .') » 


<*«C 


waitk: 

BIT 

•A.PSTAT 

«> 

BNE 

WAITK 

WRITELN( 

'  THANK 

YOU' ) ( 

(««C 

kil: 

«) 

PWAIT* 

PWAIT( 

SEND 

•3 f POWER 

(*«C 

HOV 

BBPSTAT.RI 

BIC 

•  ''0177774*R1 

CHP 

*3iRl 

«) 

BEQ 

DONE 

>TEST  KILL  CIRCUIT 
(LOOP  UNTIL  DEACTIVATED 


(TURN  ON  ELECTRONICS  t  HOTOR  POWER 


(CHECK  FOR  HP  t  EP  ON 
(HASK  OFF  HP  t  EP  STATUS  BITS 
(CHECK  IF  BOTH  BITS  ON 
(BRANCH  IF  CORRECT 


WRITELNI 

HRITELN<'  POWER  CONTROLLER  HALFUNCTION' ) ( 

ASK( 

URITELN( 

URITE( 'PLEASE  SWITCH  TO  HANUAL  HOOEr'X 
HRITELN<'  THEN  TURN  ON  YELLOW  t  GREEN  LIGHTS' )( 


(CHECK  FOR  HP  t  EP  ON 
(HASK  OFF  HP  t  EP  STATUS  BITS 
(CHECK  IF  BOTH  BITS  ON 
(LOOP  UNTIL  POWER  ON 


WRITELN( 

NRITELN<'  THANK  YOU'X 

<*«c  done: 

«) 


END(  /«  TURNON  «/ 


(««C 

waitp: 

HOV 

e*PSTAT*Rl 

BIC 

*••0177774. RI 

CHP 

•  3.R1 

«> 

BNE 

WAITP 

/*  */ 

/*  testom:  */ 

/*  (TEST  f  TURN  ON)  IS  AN  INTERACTIVE  RROCEOURE  UHICH  SHOULD  BE  «/ 

/«  CALLED  AT  THE  START  OF  EVERY  REAL-TIME  HEXAPOD  CONTROL  PROGRAM. 

/«  IT  AUTOMATICALLY  TESTS  DATA  LINK  OPERATION  AND  THEN  TURNS  ON  «/ 

/«  THE  POWER  SYSTEMS  IN  THE  PROPER  SEQUENCE.  ANY  ABNORMAL  */ 

/*  CONDITIONS  ARE  REPORTED  TO  THE  OPERATOR.  «/ 

/*  »/ 


/%%***m%%tt***%ttt*t%*tt*t****%*tt%t%ttttt**ttt**t%ttt*t*t**tt*t**%t*tt*/ 

PROCEDURE  TESTONI 

VAR  !• 

Ni 

DATAt 
LOC: 

INTEGERi 

(««C  ADBASE-''0146000 

0ABASE-''0144200 
DABUF-''0144234 
C0HSTATa''0144244 

PSTAT»‘'0144I74 
P0UER>''0144244 

.MACRO  SEND  YiZ>?T 

t:  tstb  comstat 

BPL  T 

MOV  Y 1 2 

.ENOM 

<) 

BEGIN 


(BASE  ADDRESS  OR  A/D'S 
I  BASE  ADDRESS  OF  D/A'S 
>D/A  CHANNEL  USED  FOR  FORWARD  TESTS 
(ADDRESS  OF  FORWARD  STATUS  WORD 
(  AND  MODE  COMMAND  WORD 
(ADDRESS  OF  POWER  STATUS  WORD 
(ADDRESS  OF  POWER  COMMAND  WORD 


(CHECK  IF  FORWARD  PATH  BUSY 
(SEND  DATA 


WRITELNI 

WRITE( 'PLEASE  TURN  ON  DATA  LINK  POWER  (RED  LICHTJt'X 

HRITELN('  THEN  ENTER  A  CARRIAGE  RETURN.' >i 

READLN( 


(«««««*«  FEEDBACK  TEST  ROUTINE  «*»»«$*) 

URITELN('  PERFORMING  DATA  LINK  FEEDBACK  TEST'X 

(««C  SEND  *Of POWER  (TURN  OFF  POWER  SYSTEMS 

SEND  «IrCOHSTAT  (COMMAND  TEST  MODE  *1 

«> 
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PUAITI 

PUAlTt 

n:-oi 

FOR  i:-t  TO  (HAXINT  DIU  &4>  DO 


BEGIN 

<**C 

NOW 

*''077»R1 

UNITIALIZE  ADDRESS  COUNTER 

loop: 

A8L 

Rl 

ICONVERT  TO  WORD  ADDRESS 

HOV 

ADBA8E(R1)>R2 

IFETCH  DATA 

ASR 

Rl 

IRESTDRE  COUNTER 

HOU 

R1>R3 

IGENERATE  EXPECTED  DATA  IN  R3 

SWAB 

R3 

1  BY  DUPLICATING  BITS  0-3 

ASR 

R3 

1  IN  BITS  6-9 

ASR 

R3 

ADD 

R1>R3 

BIC 

•''017&000>R3 

I  DATA  NOW  GENERATED 

CMP 

R2>R3 

ICHECK  IF  DATA  IS  CORRECT 

BEQ 

CONT 

«) 

MOV 

RIILOCCSP) 

I  SAVE  ADDRESS  OF  BAD  DATA 

N:<N41) 
IF  N<20 

THEN 

BEGIN 

URITEi'  ERROR  DETECTED  ON  LOOP'.It'i  ADDRESS' rLOC) » 

URITELNI 

ENDi 

(««C 

CONTS  DEC  RI  IDECREHENT  ADDRESS  COUNTER 

BPL  LOOP  »LOOP  UNTIL  RI<0 

*) 

END#  /*  FOR  I  */ 

IF  N  <>  0  THEN 
BEGIN 
URITELNi 

URITELN<Ni'  TRAN8NISSI0N  ERRORS  DETECTED  IN'>HAXINT»'  TESTS.')# 
ASK) 

ENDI 


(««*««««  FEEDFORWARD  TEST  ROUTINE  «««««$*> 

HRITELNI 

URITELNt'  PERFORNINO  DATA  LINK  FEEDFORWARD  TEST') I 

(««C  SEND  •2rCONSTAT  ICONHAND  TEST  NODE  *2 

«) 
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n:>oi 

PUAITI 


data:-o  to 

BEGIN 

127  DO 

(*«C 

«) 

SEND 

DATAISP) fDABUF 

ISEND  DATA 

PUAITI 

(»tC 

MOV 

BBADBASEfRl 

IREAD  BACK  DATA 

CMPB 

R1>DATA(SP> 

ICOHPARE  DATA  SENT  t  RECIEVED 

DEO 

GOOD 

•BRANCH  IF  DATA  GOOD 

INC 

N(SP) 

•INCREMENT  ERROR  COUNTER 

good; 

CON 

DATAiSPI 

•FLIP  BITS  FOR  NEXT  OUTPUT 

«) 

PUAITI 

SEND 

OATA(SP) fDABUF 

ISEND  DATA 

<««C 

NOV 

••ADBASEiRl 

IREAD  BACK  DATA 

CMPB 

RlfDATAISP) 

{COMPARE  DATA  SENT  t  RECIEVED 

BEO 

000D2 

•BRANCH  IF  DATA  GOOD 

INC 

N<SP) 

I  INCREMENT  ERROR  COUNTER 

G00D2S 

«) 

COM 

OATA(SP) 

•RESTORE  DATA 

ENOI 

/»  FOR 

DATA  «/ 

IF  M  <>  0  THCN 
BEGIN 
URITELNI 

URITEL.N<N>'  TRANSMISSION  ERRORS  DETECTED  IN  256  TESTS. ')» 

ASK  I 

ENDI 


POUER  CONTROLLER  MODE  TEST  «»«««*»> 

<«*C  SEND  BOfCONSTAT  ICOHHAND  NORMAL  MODE 

«) 

PUAITI 

PUAIT* 

(**C  TSTB  PST AT  »TEST  IF  POUER  CONTROLLER  IN  COMPUTER  MODE 
BNI  COMP  ^BRANCH  IF  IT  IS 

«> 

URITELNI 

URITELNI 'PLEASE  PLACE  POUER  CONTROLLER  IN  COMPUTER  MODE.'II 
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(*«C 

UAITC:  TSTB  PSTAT  (TEST  FOR  COHPUTER  MODE 

BPL  UAITC  »LOOP  UNTIL  NODE  CHANGED 

*) 

UR1TELN('  THANK  YOU')* 

(*«C  COHPt 

«) 


(«««««««  ZERO  OAC  ROUTINE  «»«•**»> 


•  TO  17 
BEGIN 

DO 

(««C 

NOV 

I(SP)>R1 

IHOVE  1  TO 

R1 

ASL 

R1 

1  CONVERT  TO 

1  UORD 

ADDRESS 

«) 

ENDI 

SEND 

•''0177>DABA8E(R1) 

ISEND  ZERO 

VOLTS 

TO  JOINT 

TURNON i 
URITELNI 

URITELNi'  INITIALIZATION  COHPLETE'>» 
URITELNI 

ENDI  /t  TESTON  t/ 
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file:  KYCK34.HAC  «•*«««/ 


PROORAHHERS:  CHARLES  KLEINt  DENNIS  PUSH 
date:  20-APR-82 

GLOBAL  variables:  NONE 

purpose:  assenbler  level  keyboard  input  for  special-purpose 

USE  UITH  HIGH  LEVEL  LANGUAGES. 

SUBROUTINES  CONTAINED: 

KEYINT:  INITIALIZES  TERHINAL  CHARACTERISTICS  AND  PLACES  AN 
INPUT  REQUEST  ON  THE  SYSTEM  QUEUE.  KEYINT  SHOULD  BE 
CALLED  ONLY  ONCE.  NO  ARGUMENTS  ARE  REQUIRED. 

KEYCK:  returns  the  character  typed  ON  THE  KEYBOARD  IN  ITS 

ONE  ARGUMENT  (VARIABLE  TYPE  CHAR).  IF  NO  CHARACTER  HAS 
BEEN  INPUT  SINCE  THE  LAST  CALL.  A  NULL  CHARACTER  IS 
RETURNED  (ASCII  0).  SAMPLE  CALL:  KEYCK ( CHARACTER > » 

kyuait:  is  to  be  called  when  it  is  desired  to  suspend  program 

EXECUTION  UNTIL  A  CHARACTER  IS  INPUT.  THUS  FREEING  THE 
SYSTEM  FOR  OTHER  TASKS.  EXECUTION  IS  RESUMED  UHEN  AN 
INPUT  IS  RECIEVEO.  KEYCK  CAN  THEN  BE  USED  TO  FETCH  THE 
CHARACTER.  NO  ARGUMENTS  ARE  REQUIRED. 

IOKILL:  is  USED  TO  CANCEL  THE  INPUT  REQUEST  WHEN  A  HIGH-LEVEL 
READ  (READLN)  IS  TO  BE  PERFORMED.  NO  ARGUMENTS  ARE 
REQUIRED. 

INQUE:  is  called  to  reassert  the  input  REQUEST  AFTER  AN  IOKILL 
AND  HIGH-LEVEL  READ.  NO  ARGUMENTS  ARE  REQUIRED. 


««*  ALL  SUBROUTINES  ARE  IN  FORTRAN  FORMAT.  *«« 


.LIST  TTM 

.MCALL  QIO»S.UTSE«S.ALUNtS 

LOCAL  SYMBOL  DEFINITION 
LUN2  >  2 

EFNI  -  11 

EFN2  -  12 
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(  LOCAL 
i 

DATA  BLOCKS! 

.PSECT 

DATA. Dr RH 

Nso: 

.BLKB 

.EVEN 

1 

(CHARACTER  BUFFER 

TCHARS 

.BYTE 

TC.FOXf 1 

(TERHINAL  CHARACTERISTICS 

iost: 

1 

I 

keyint: 

.BLKU 

.PSECT 

• 

« 

2 

(I/O  STATUS  RETURN  LOCATION 

ALUN«S 

•LUN2r**TI 

(ASSIGN  LUN  TO  TERHINAL 

QlOtS 

•I0.ATTt«LUN2 

(ATTACH  TERHINAL 

QlOtS 

•SF.SHCr*LUN2r  r  t  r  t <«TCHAR r *2> 

(SET  TO  FULL  DUPLEX 

QlOtS 

»ia.RNEr«LUN2f«EFN2r rtlOSTr  r<*HSOr*l> 

(ASSERT  INPUT  REQUEST 

BCS 

ERROR 

(SIGNAL  IF  ERROR 

( 

RTS 

PC 

( 

KEYCKC 

TST 

<R5)  + 

(POINT  R5  TO  ARQUHENT 

CLRB 

•0<RS) 

(PUT  NULL  IN  ARQUHENT 

CNPB 

•IS.SUCrIOST 

(TEST  FOR  SUCCESSFUL  READ 

BNE 

RETURN 

(RETURN  IF  NOT 

NOVB 

N&Gt«0<RS) 

(PUT  CHAR.  IN  ARQUHENT 

inque:  : 

QlOtS 

•  I0.RNEt*LUN2r«EFN2r  rBIOSTr  r<*HS6r»l> 

(REASSERT  INPUT  REQUEST 

BCS 

ERROR 

(SIGNAL  IF  ERROR 

return: 

RTS 

PC 

error: 

NOV  *2i 
lOT 

RO 

(SET  ERROR  FLAG 

fCYVAXTt  t 

UTSEtS  terNS  »uait  for  keyboard  inrut  with  little  overhead 

j  RTS  PC 

* 

lOKlLLIi 

IQIOfS  IO.KIL>«LUN2>«EFNl  (CANCEL  I/O  REQUESTS 

UTSEtS  BEFNl  (WAIT  FOR  COHPLETION 

RTS  PC 

I 

H  .END 

I 

I 

I 
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FILEt  CHPL34.CHD 


I  FUNCTION!  INDIRECT  CONNAND  FILE  TO 
i  COMPILE  t  ASSEMBLE  ALL  FILES 

i  OF  ROBOT  3.4 

I 

PAS  R0BT34-0BLF34tR0BT34 

MAC  R0BT34-R0BT34 

PAS  PLAN34-GBLF34tPLAN34 

MAC  PLAN34-PLAN34 

PAS  F00T34-GBLF34fF00T34 

MAC  F00T34-F00T34 

PAS  SERV34>GBLF34«SERV34 

MAC  SERU34-SERW34 

PAS  1NIT34-GBLF34«INIT34 

MAC  1NIT34-INIT34 

PAS  L1NE34-GBLF34.LINE34 

MAC  LINE34>L1NC34 

PAS  LIBR34-GBLF34tLIBR34 

MAC  L1BR34-L1BR34 

PAS  DLNK34-0BLF34>DLNK34 

MAC  DLNK34-DLNK34 

PAS  POMR34-POUR34 

MAC  POUR34-paUR34 

MAC  KYCK34-KYCK34 

TKB  0R0BT34 
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J 


»  FlLEt  R0gT34.CHD 

I  fumctioh:  provides  linkage  INPORHATION 

J  rOR  THE  TASK  BUILDER 

R0BT34/FP,R0BT34/CR/-SP-R0BT34 

PLAN34 

F00T34 

LIBR34 

SERV34 

DLNK34 

INIT34 

LINE34 

KrCK34 

P0UR34 

CltllPASLlB/LB 

/ 

CONHON-IOPAGESRW 

tMt*ct-««h««p:sooo 

UNITS-7 

// 


I 

I 

I 
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